From 1a8ba70cd8fb39f62f4f63df19d8467d359780e7 Mon Sep 17 00:00:00 2001 From: IamPete1 <33176108+IamPete1@users.noreply.github.com> Date: Fri, 5 Apr 2019 19:28:09 +0100 Subject: [PATCH 1/4] SITL: add copter tailsitter --- libraries/SITL/SIM_Plane.h | 1 + libraries/SITL/SIM_QuadPlane.cpp | 16 ++++++++++++++-- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h index b5db14f3ea2a5..bfd0862e8e6f8 100644 --- a/libraries/SITL/SIM_Plane.h +++ b/libraries/SITL/SIM_Plane.h @@ -99,6 +99,7 @@ class Plane : public Aircraft { bool reverse_elevator_rudder; bool ice_engine; bool tailsitter; + bool copter_tailsitter; bool have_launcher; float launch_accel; float launch_time; diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 371d914e9be56..37468a9b20e53 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -28,7 +28,9 @@ QuadPlane::QuadPlane(const char *frame_str) : // default to X frame const char *frame_type = "x"; uint8_t motor_offset = 4; - + + ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; + if (strstr(frame_str, "-octa-quad")) { frame_type = "octa-quad"; } else if (strstr(frame_str, "-octaquad")) { @@ -67,6 +69,11 @@ QuadPlane::QuadPlane(const char *frame_str) : frame_type = "tilttri"; // fwd motor gives zero thrust thrust_scale = 0; + } else if (strstr(frame_str, "-copter_tailsitter")) { + frame_type = "+"; + copter_tailsitter = true; + ground_behavior = GROUND_BEHAVIOR_TAILSITTER; + thrust_scale *= 1.5; } frame = Frame::find_frame(frame_type); if (frame == nullptr) { @@ -94,7 +101,6 @@ QuadPlane::QuadPlane(const char *frame_str) : mass = frame->get_mass() * 1.5; frame->set_mass(mass); - ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; lock_step_scheduled = true; } @@ -116,6 +122,12 @@ void QuadPlane::update(const struct sitl_input &input) frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1], false); + // rotate frames for copter tailsitters + if (copter_tailsitter) { + quad_rot_accel.rotate(ROTATION_PITCH_270); + quad_accel_body.rotate(ROTATION_PITCH_270); + } + // estimate voltage and current frame->current_and_voltage(battery_voltage, battery_current); From 0a0fd9c2412976f4dedc66483d56751e0abe5c55 Mon Sep 17 00:00:00 2001 From: IamPete1 <33176108+IamPete1@users.noreply.github.com> Date: Fri, 5 Apr 2019 19:28:41 +0100 Subject: [PATCH 2/4] Tools: autotest: add copter tailsitter and params --- .../quadplane-copter_tailsitter.parm | 37 +++++++++++++++++++ Tools/autotest/pysim/vehicleinfo.py | 4 ++ 2 files changed, 41 insertions(+) create mode 100644 Tools/autotest/default_params/quadplane-copter_tailsitter.parm diff --git a/Tools/autotest/default_params/quadplane-copter_tailsitter.parm b/Tools/autotest/default_params/quadplane-copter_tailsitter.parm new file mode 100644 index 0000000000000..6cafdcb50c220 --- /dev/null +++ b/Tools/autotest/default_params/quadplane-copter_tailsitter.parm @@ -0,0 +1,37 @@ +Q_ENABLE 1 +Q_FRAME_CLASS 1 +Q_FRAME_TYPE 0 +Q_TAILSIT_MOTMX 3 +Q_A_ACCEL_P_MAX 207069.531250 +Q_A_ACCEL_R_MAX 117885.953125 +Q_A_ACCEL_Y_MAX 12287.541992 +Q_A_ANGLE_BOOST 1.000000 +Q_A_ANG_LIM_TC 1.000000 +Q_A_ANG_PIT_P 18.000000 +Q_A_ANG_RLL_P 3.455029 +Q_A_ANG_YAW_P 2.068653 +Q_A_INPUT_TC 0.200000 +Q_A_RATE_FF_ENAB 1.000000 +Q_A_RATE_P_MAX 0.000000 +Q_A_RATE_R_MAX 0.000000 +Q_A_RATE_Y_MAX 0.000000 +Q_A_RAT_PIT_D 0.004973 +Q_A_RAT_PIT_FF 0.000000 +Q_A_RAT_PIT_FILT 10.000000 +Q_A_RAT_PIT_I 0.528246 +Q_A_RAT_PIT_IMAX 0.500000 +Q_A_RAT_PIT_P 0.528246 +Q_A_RAT_RLL_D 0.001000 +Q_A_RAT_RLL_FF 0.000000 +Q_A_RAT_RLL_FILT 10.000000 +Q_A_RAT_RLL_I 0.541613 +Q_A_RAT_RLL_IMAX 0.500000 +Q_A_RAT_RLL_P 0.541613 +Q_A_RAT_YAW_D 0.000000 +Q_A_RAT_YAW_FF 0.000000 +Q_A_RAT_YAW_FILT 1.073194 +Q_A_RAT_YAW_I 0.039192 +Q_A_RAT_YAW_IMAX 0.500000 +Q_A_RAT_YAW_P 0.391919 +Q_TAILSIT_THSCMX 1.5 +Q_TRANS_DECEL 8 diff --git a/Tools/autotest/pysim/vehicleinfo.py b/Tools/autotest/pysim/vehicleinfo.py index c5d1e9da8133b..b99c15c574aa1 100644 --- a/Tools/autotest/pysim/vehicleinfo.py +++ b/Tools/autotest/pysim/vehicleinfo.py @@ -262,6 +262,10 @@ def __init__(self): "waf_target": "bin/arduplane", "default_params_filename": ["default_params/plane.parm", "default_params/plane-jet.parm"], }, + "quadplane-copter_tailsitter": { + "waf_target": "bin/arduplane", + "default_params_filename": ["default_params/quadplane.parm","default_params/quadplane-copter_tailsitter.parm"], + }, "plane": { "waf_target": "bin/arduplane", "default_params_filename": "default_params/plane.parm", From 4f327dadf2204156b9dd9f3369bb131360a95809 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 22 Oct 2021 23:57:06 +0100 Subject: [PATCH 3/4] SITL: update tailsitter ground behavoir --- libraries/SITL/SIM_Aircraft.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index b897694747fae..ddf6f910fa581 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -690,15 +690,24 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel) break; } case GROUND_BEHAVIOR_TAILSITTER: { - // point straight up + // rotate normal refernce frame to get yaw angle, then rotate back + Matrix3f rot; + rot.from_rotation(ROTATION_PITCH_270); float r, p, y; - dcm.to_euler(&r, &p, &y); + (dcm * rot).to_euler(&r, &p, &y); y = y + yaw_rate * delta_time; - dcm.from_euler(0.0f, radians(90), y); + dcm.from_euler(0.0, 0.0, y); + rot.from_rotation(ROTATION_PITCH_90); + dcm *= rot; // X, Y movement tracks ground movement velocity_ef.x = gnd_movement.x; velocity_ef.y = gnd_movement.y; + if (velocity_ef.z > 0.0f) { + velocity_ef.z = 0.0f; + } gyro.zero(); + gyro.x = yaw_rate; + use_smoothing = true; break; } } From df2a12128c822bcf784de9b413d1369b6ee71d07 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 25 Oct 2021 01:36:05 +0100 Subject: [PATCH 4/4] Tools: autotest: quadplane: reboot for tailsitters and allow longer time for disarm --- Tools/autotest/arduplane.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index fc34d036d2108..10a540b79ef47 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -534,7 +534,7 @@ def fly_mission(self, filename, mission_timeout=60.0, strict=True, quadplane=Fal self.wait_waypoint(1, num_wp, max_dist=60, timeout=mission_timeout) self.wait_groundspeed(0, 0.5, timeout=mission_timeout) if quadplane: - self.wait_statustext("Throttle disarmed", timeout=70) + self.wait_statustext("Throttle disarmed", timeout=200) else: self.wait_statustext("Auto disarmed", timeout=60) self.progress("Mission OK") @@ -2993,6 +2993,10 @@ def fly_each_frame(self): quadplane = self.get_parameter('Q_ENABLE') if quadplane: mission_file = "basic-quadplane.txt" + tailsitter = self.get_parameter('Q_TAILSIT_ENABLE') + if tailsitter: + # tailsitter needs extra re-boot to pick up the rotated AHRS view + self.reboot_sitl() self.wait_ready_to_arm() self.arm_vehicle() self.fly_mission(mission_file, strict=False, quadplane=quadplane, mission_timeout=400.0)