From 3d2a460e3532a22d932d61997bce33af62699984 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 3 Nov 2019 16:34:10 -0700 Subject: [PATCH 1/4] RC_Channel: add RC option for VTOL manual forward throttle --- libraries/RC_Channel/RC_Channel.cpp | 2 +- libraries/RC_Channel/RC_Channel.h | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 4e4d2c5a8f84b..b13e15620a5b8 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -92,7 +92,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Description: Function assigned to this RC channel // @Values{Copter}: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 37:Throw, 38:ADSB Avoidance En, 39:PrecLoiter, 40:Proximity Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 46:RC Override Enable, 47:User Function 1, 48:User Function 2, 49:User Function 3, 52:Acro, 55:Guided, 56:Loiter, 57:Follow, 58:Clear Waypoints, 60:ZigZag, 61:ZigZag SaveWP, 62:Compass Learn, 65:GPS Disable, 66:Relay5, 67:Relay6, 68:Stabilize, 69:PosHold, 70:AltHold, 71:FlowHold, 72:Circle, 73:Drift, 76:Standby Mode, 78:RunCam Control, 79:RunCam OSD Control, 100:KillIMU1, 101:KillIMU2 // @Values{Rover}: 0:Do Nothing, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 11:Fence, 16:Auto, 19:Gripper, 24:Auto Mission Reset, 28:Relay On/Off, 30:Lost Rover Sound, 31:Motor Emergency Stop, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 40:Proximity Avoidance, 41:ArmDisarm, 42:SmartRTL, 46:RC Override Enable, 50:LearnCruise, 51:Manual, 52:Acro, 53:Steering, 54:Hold, 55:Guided, 56:Loiter, 57:Follow, 58:Clear Waypoints, 59:Simple Mode, 62:Compass Learn, 63:Sailboat Tack, 65:GPS Disable, 66:Relay5, 67:Relay6, 74:Sailboat motoring 3pos, 78:RunCam Control, 79:RunCam OSD Control, 100:KillIMU1, 101:KillIMU2, 207:MainSail - // @Values{Plane}: 0:Do Nothing, 4:ModeRTL, 9:Camera Trigger, 16:ModeAuto, 24:Auto Mission Reset, 28:Relay On/Off, 29:Landing Gear, 34:Relay2 On/Off, 30:Lost Plane Sound, 31:Motor Emergency Stop, 35:Relay3 On/Off, 36:Relay4 On/Off, 38:ADSB Avoidance En, 41:ArmDisarm, 43:InvertedFlight, 46:RC Override Enable, 51:ModeManual, 55:ModeGuided, 56:ModeLoiter, 58:Clear Waypoints, 62:Compass Learn, 64:Reverse Throttle, 65:GPS Disable, 66:Relay5, 67:Relay6, 72:ModeCircle, 77:ModeTakeoff, 78:RunCam Control, 79:RunCam OSD Control, 100:KillIMU1, 101:KillIMU2, 208:Flap + // @Values{Plane}: 0:Do Nothing, 4:ModeRTL, 9:Camera Trigger, 16:ModeAuto, 24:Auto Mission Reset, 28:Relay On/Off, 29:Landing Gear, 34:Relay2 On/Off, 30:Lost Plane Sound, 31:Motor Emergency Stop, 35:Relay3 On/Off, 36:Relay4 On/Off, 38:ADSB Avoidance En, 41:ArmDisarm, 43:InvertedFlight, 46:RC Override Enable, 51:ModeManual, 55:ModeGuided, 56:ModeLoiter, 58:Clear Waypoints, 62:Compass Learn, 64:Reverse Throttle, 65:GPS Disable, 66:Relay5, 67:Relay6, 72:ModeCircle, 77:ModeTakeoff, 78:RunCam Control, 79:RunCam OSD Control, 80:fwd throttle, 100:KillIMU1, 101:KillIMU2, 208:Flap // @User: Standard AP_GROUPINFO_FRAME("OPTION", 6, RC_Channel, option, 0, AP_PARAM_FRAME_COPTER|AP_PARAM_FRAME_ROVER|AP_PARAM_FRAME_PLANE), diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index c05d0202954b1..42a61aa52e4d4 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -177,6 +177,7 @@ class RC_Channel { TAKEOFF = 77, // takeoff RUNCAM_CONTROL = 78, // control RunCam device RUNCAM_OSD_CONTROL = 79, // control RunCam OSD + FWD_THR = 80, // VTOL manual forward throttle KILL_IMU1 = 100, // disable first IMU (for IMU failure testing) KILL_IMU2 = 101, // disable second IMU (for IMU failure testing) // if you add something here, make sure to update the documentation of the parameter in RC_Channel.cpp! From 1ac36fd63e6a5586abb39a2822c5e2b8e8153b29 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 18 May 2019 14:16:22 -0600 Subject: [PATCH 2/4] Plane: add acro and stabilized tiltrotor fwd throttle modes add rudder control for QACRO: route copter controller yaw output to plane rudder output allow forward/yaw motor tilt when disarmed add FWD_THR_CH RC option mix in forward throttle proportional to (throttle - hover throttle) offset mix from hover by +/-.25 based on FWD_THR_CH --- ArduPlane/ArduPlane.cpp | 2 + ArduPlane/Attitude.cpp | 6 +++ ArduPlane/GCS_Mavlink.cpp | 2 + ArduPlane/GCS_Plane.cpp | 2 + ArduPlane/Plane.h | 4 ++ ArduPlane/RC_Channel.cpp | 6 ++- ArduPlane/control_modes.cpp | 6 +++ ArduPlane/events.cpp | 4 ++ ArduPlane/mode.h | 40 +++++++++++++++++ ArduPlane/mode_qacro.cpp | 3 ++ ArduPlane/mode_qafthr.cpp | 12 ++++++ ArduPlane/mode_qsfthr.cpp | 12 ++++++ ArduPlane/quadplane.cpp | 86 ++++++++++++++++++++++++++++++------- ArduPlane/quadplane.h | 9 +++- ArduPlane/radio.cpp | 2 + ArduPlane/servos.cpp | 2 +- ArduPlane/tiltrotor.cpp | 40 ++++++++++++----- 17 files changed, 210 insertions(+), 28 deletions(-) create mode 100644 ArduPlane/mode_qafthr.cpp create mode 100644 ArduPlane/mode_qsfthr.cpp diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index ae023d102e3d3..66c484b385f1a 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -526,6 +526,8 @@ void Plane::update_navigation() case Mode::Number::QRTL: case Mode::Number::QAUTOTUNE: case Mode::Number::QACRO: + case Mode::Number::QACRO_FTHR: + case Mode::Number::QSTAB_FTHR: // nothing to do break; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 465eb61da687c..19ba64928cd33 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -144,6 +144,8 @@ void Plane::stabilize_stick_mixing_direct() control_mode == &mode_qland || control_mode == &mode_qrtl || control_mode == &mode_qacro || + control_mode == &mode_qafthr || + control_mode == &mode_qsfthr || control_mode == &mode_training || control_mode == &mode_qautotune) { return; @@ -175,6 +177,8 @@ void Plane::stabilize_stick_mixing_fbw() control_mode == &mode_qland || control_mode == &mode_qrtl || control_mode == &mode_qacro || + control_mode == &mode_qafthr || + control_mode == &mode_qsfthr || control_mode == &mode_training || control_mode == &mode_qautotune || (control_mode == &mode_auto && g.auto_fbw_steer == 42)) { @@ -403,6 +407,8 @@ void Plane::stabilize() control_mode == &mode_qland || control_mode == &mode_qrtl || control_mode == &mode_qacro || + control_mode == &mode_qafthr || + control_mode == &mode_qsfthr || control_mode == &mode_qautotune) && !quadplane.in_tailsitter_vtol_transition()) { quadplane.control_run(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 348875082d18a..2dcb0def00a4f 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -24,6 +24,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const case Mode::Number::TRAINING: case Mode::Number::ACRO: case Mode::Number::QACRO: + case Mode::Number::QACRO_FTHR: _base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case Mode::Number::STABILIZE: @@ -31,6 +32,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const case Mode::Number::AUTOTUNE: case Mode::Number::FLY_BY_WIRE_B: case Mode::Number::QSTABILIZE: + case Mode::Number::QSTAB_FTHR: case Mode::Number::QHOVER: case Mode::Number::QLOITER: case Mode::Number::QLAND: diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index b513b1d087c62..3e708d62a17ad 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -46,6 +46,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) case Mode::Number::ACRO: case Mode::Number::QACRO: + case Mode::Number::QACRO_FTHR: rate_controlled = true; break; @@ -53,6 +54,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) case Mode::Number::FLY_BY_WIRE_A: case Mode::Number::AUTOTUNE: case Mode::Number::QSTABILIZE: + case Mode::Number::QSTAB_FTHR: case Mode::Number::QHOVER: case Mode::Number::QLAND: case Mode::Number::QLOITER: diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index f07598eb825f0..82693034b31ae 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -155,6 +155,8 @@ class Plane : public AP_Vehicle { friend class ModeQLand; friend class ModeQRTL; friend class ModeQAcro; + friend class ModeQAFTHR; + friend class ModeQSFTHR; friend class ModeQAutotune; friend class ModeTakeoff; @@ -280,6 +282,8 @@ class Plane : public AP_Vehicle { ModeQLand mode_qland; ModeQRTL mode_qrtl; ModeQAcro mode_qacro; + ModeQAFTHR mode_qafthr; + ModeQSFTHR mode_qsfthr; ModeQAutotune mode_qautotune; ModeTakeoff mode_takeoff; diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index b8d2c33048450..f67939b52ec96 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -68,6 +68,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, case AUX_FUNC::MANUAL: case AUX_FUNC::RTL: case AUX_FUNC::TAKEOFF: + case AUX_FUNC::FWD_THR: break; case AUX_FUNC::REVERSE_THROTTLE: @@ -85,7 +86,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, // handle in parent class RC_Channel::init_aux_function(ch_option, ch_flag); break; -} + } } // do_aux_function - implement the function invoked by auxillary switches @@ -132,6 +133,9 @@ void RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const aux_swi case AUX_FUNC::FLAP: break; // flap input label, nothing to do + case AUX_FUNC::FWD_THR: + break; + default: RC_Channel::do_aux_function(ch_option, ch_flag); break; diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 33a0677b67060..850c619194627 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -67,6 +67,12 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num) case Mode::Number::QACRO: ret = &mode_qacro; break; + case Mode::Number::QACRO_FTHR: + ret = &mode_qafthr; + break; + case Mode::Number::QSTAB_FTHR: + ret = &mode_qsfthr; + break; case Mode::Number::QAUTOTUNE: ret = &mode_qautotune; break; diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 3a8cadaad07bb..329c277def9d5 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -30,6 +30,8 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso case Mode::Number::QHOVER: case Mode::Number::QAUTOTUNE: case Mode::Number::QACRO: + case Mode::Number::QACRO_FTHR: + case Mode::Number::QSTAB_FTHR: failsafe.saved_mode_number = control_mode->mode_number(); failsafe.saved_mode_set = true; if (quadplane.options & QuadPlane::OPTION_FS_QRTL) { @@ -98,6 +100,8 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason case Mode::Number::QHOVER: case Mode::Number::QLOITER: case Mode::Number::QACRO: + case Mode::Number::QACRO_FTHR: + case Mode::Number::QSTAB_FTHR: case Mode::Number::QAUTOTUNE: if (quadplane.options & QuadPlane::OPTION_FS_QRTL) { set_mode(mode_qrtl, reason); diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 169000efe15c3..e2cde40bb1c93 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -38,6 +38,8 @@ class Mode QRTL = 21, QAUTOTUNE = 22, QACRO = 23, + QACRO_FTHR = 24, + QSTAB_FTHR = 25, }; // Constructor @@ -420,6 +422,8 @@ class ModeQAcro : public Mode { public: + friend class ModeQAFTHR; + Number mode_number() const override { return Number::QACRO; } const char *name() const override { return "QACO"; } const char *name4() const override { return "QACRO"; } @@ -480,3 +484,39 @@ class ModeTakeoff: public Mode bool _enter() override; }; + +class ModeQAFTHR : public Mode +{ +public: + + Number mode_number() const override { return Number::QACRO_FTHR; } + const char *name() const override { return "QACRO_FTHR"; } + const char *name4() const override { return "QAFT"; } + + bool is_vtol_mode() const override { return true; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeQSFTHR : public Mode +{ +public: + + Number mode_number() const override { return Number::QSTAB_FTHR; } + const char *name() const override { return "QSTAB_FTHR"; } + const char *name4() const override { return "QSFT"; } + + bool is_vtol_mode() const override { return true; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp index 370d458a680a9..b9a5a7c8d3322 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -21,6 +21,9 @@ void ModeQAcro::update() Vector3f att_target = plane.quadplane.attitude_control->get_att_target_euler_cd(); plane.nav_pitch_cd = att_target.y; plane.nav_roll_cd = att_target.x; + + // set rudder using copter controller + plane.steering_control.rudder = plane.quadplane.motors->get_yaw() * SERVO_MAX; return; } diff --git a/ArduPlane/mode_qafthr.cpp b/ArduPlane/mode_qafthr.cpp new file mode 100644 index 0000000000000..1e772875e6a8c --- /dev/null +++ b/ArduPlane/mode_qafthr.cpp @@ -0,0 +1,12 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeQAFTHR::_enter() +{ + return plane.mode_qacro._enter(); +} + +void ModeQAFTHR::update() +{ + return plane.mode_qacro.update(); +} diff --git a/ArduPlane/mode_qsfthr.cpp b/ArduPlane/mode_qsfthr.cpp new file mode 100644 index 0000000000000..8925f3e5b3b16 --- /dev/null +++ b/ArduPlane/mode_qsfthr.cpp @@ -0,0 +1,12 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeQSFTHR::_enter() +{ + return plane.mode_qstabilize._enter(); +} + +void ModeQSFTHR::update() +{ + return plane.mode_qstabilize.update(); +} diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 93509905ebe38..93a7e859e05ba 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -484,6 +484,13 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @User: Standard AP_GROUPINFO("TAILSIT_GSCMIN", 18, QuadPlane, tailsitter.gain_scaling_min, 0.4), + // @Param: FWD_THR_MIX + // @DisplayName: Throttle to VTOL forward throttle mix + // @Description: Amount of forward throttle applied when above hover throttle. + // @Range 0 1 + // @RebootRequired: False + AP_GROUPINFO("FWD_THR_MIX", 19, QuadPlane, fwd_thr_mix, 0.5), + AP_GROUPEND }; @@ -651,6 +658,9 @@ bool QuadPlane::setup(void) break; } + // rc_fwd_thr_ch will be null if no channel is assigned + rc_fwd_thr_ch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FWD_THR); + if (tailsitter.motor_mask == 0) { // this is a normal quadplane switch (motor_class) { @@ -1186,6 +1196,12 @@ bool QuadPlane::is_flying_vtol(void) const if (plane.control_mode == &plane.mode_qacro) { return true; } + if (plane.control_mode == &plane.mode_qafthr) { + return true; + } + if (plane.control_mode == &plane.mode_qsfthr) { + return true; + } if (plane.control_mode == &plane.mode_guided && guided_takeoff) { return true; } @@ -1999,9 +2015,11 @@ void QuadPlane::control_run(void) switch (plane.control_mode->mode_number()) { case Mode::Number::QACRO: + case Mode::Number::QACRO_FTHR: control_qacro(); break; case Mode::Number::QSTABILIZE: + case Mode::Number::QSTAB_FTHR: control_stabilize(); break; case Mode::Number::QHOVER: @@ -2025,7 +2043,8 @@ void QuadPlane::control_run(void) // we also stabilize using fixed wing surfaces float speed_scaler = plane.get_speed_scaler(); - if (plane.control_mode->mode_number() == Mode::Number::QACRO) { + if (plane.control_mode->mode_number() == Mode::Number::QACRO || + plane.control_mode->mode_number() == Mode::Number::QACRO_FTHR) { plane.stabilize_acro(speed_scaler); } else { plane.stabilize_roll(speed_scaler); @@ -2050,6 +2069,7 @@ bool QuadPlane::init_mode(void) switch (plane.control_mode->mode_number()) { case Mode::Number::QSTABILIZE: + case Mode::Number::QSTAB_FTHR: init_stabilize(); break; case Mode::Number::QHOVER: @@ -2072,6 +2092,7 @@ bool QuadPlane::init_mode(void) return qautotune.init(); #endif case Mode::Number::QACRO: + case Mode::Number::QACRO_FTHR: init_qacro(); break; default: @@ -2164,6 +2185,8 @@ bool QuadPlane::in_vtol_mode(void) const plane.control_mode == &plane.mode_qland || plane.control_mode == &plane.mode_qrtl || plane.control_mode == &plane.mode_qacro || + plane.control_mode == &plane.mode_qafthr || + plane.control_mode == &plane.mode_qsfthr || plane.control_mode == &plane.mode_qautotune || ((plane.control_mode == &plane.mode_guided || plane.control_mode == &plane.mode_avoidADSB) && plane.auto_state.vtol_loiter) || in_vtol_auto()); @@ -2824,24 +2847,57 @@ void QuadPlane::Log_Write_QControl_Tuning() reduces the need for down pitch which reduces load on the vertical lift motors. */ -int8_t QuadPlane::forward_throttle_pct(void) +int8_t QuadPlane::forward_throttle_pct(bool tiltrotor) { + // allow tiltrotor motors to tilt in response to forward throttle when disarmed + if (!tiltrotor) { + // disable forward motor if not in a VTOL mode + if (!in_vtol_mode()) { + return 0; + } + // disable forward motor if disarmed or spooled down + if (!motors->armed() || + motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::GROUND_IDLE) { + return 0; + } + } /* - in non-VTOL modes or modes without a velocity controller. We - don't use it in QHOVER or QSTABILIZE as they are the primary - recovery modes for a quadplane and need to be as simple as - possible. They will drift with the wind + In QxFTHR modes, mix throttle to fwd throttle. */ - if (!in_vtol_mode() || - !motors->armed() || - vel_forward.gain <= 0 || - plane.control_mode == &plane.mode_qstabilize || - plane.control_mode == &plane.mode_qhover || - plane.control_mode == &plane.mode_qautotune || - motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::GROUND_IDLE) { + if (plane.control_mode == &plane.mode_qafthr || + plane.control_mode == &plane.mode_qsfthr) { + + float offset = 0; + if (rc_fwd_thr_ch != nullptr) { + // set tilt zero offset from rc_fwd_thr_ch + offset = 0.25f * rc_fwd_thr_ch->norm_input(); + } + + float fwd_thr = 0; + if (!is_zero(fwd_thr_mix)) { + // calculate mix proportional to throttle above hover + // *** this relies on hover thrust being at center stick *** + // get scaled throttle input and normalize to [0,1] + float thr = plane.channel_throttle->get_control_in(); + thr /= plane.channel_throttle->get_range(); + + // set forward throttle to thr_max * mix + fwd_thr = constrain_float(fwd_thr_mix * 2 * (thr - 0.5f + offset), 0, 1); + } + return 100.0f * fwd_thr; + } + + /* + in qautotune mode or modes without a velocity controller + */ + if (vel_forward.gain <= 0 || + plane.control_mode == &plane.mode_qautotune) { return 0; } + /* + in modes with a velocity controller + */ float deltat = (AP_HAL::millis() - vel_forward.last_ms) * 0.001f; if (deltat > 1 || deltat < 0) { vel_forward.integrator = 0; @@ -2862,10 +2918,10 @@ int8_t QuadPlane::forward_throttle_pct(void) vel_forward.integrator = 0; return 0; } + // get component of velocity error in fwd body frame direction Vector3f vel_error_body = ahrs.get_rotation_body_to_ned().transposed() * ((desired_velocity_cms*0.01f) - vel_ned); - // find component of velocity error in fwd body frame direction - float fwd_vel_error = vel_error_body * Vector3f(1,0,0); + float fwd_vel_error = vel_error_body.x; // scale forward velocity error by maximum airspeed fwd_vel_error /= MAX(plane.aparm.airspeed_max, 5); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 84cf31bcdcef3..15ab2ef2da5d8 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -36,6 +36,8 @@ class QuadPlane friend class ModeQStabilize; friend class ModeQAutotune; friend class ModeQAcro; + friend class ModeQAFTHR; + friend class ModeQSFTHR; QuadPlane(AP_AHRS_NavEKF &_ahrs); @@ -100,7 +102,7 @@ class QuadPlane } // return desired forward throttle percentage - int8_t forward_throttle_pct(void); + int8_t forward_throttle_pct(bool tiltrotor); float get_weathervane_yaw_rate_cds(void); // see if we are flying from vtol point of view @@ -321,6 +323,11 @@ class QuadPlane // manual throttle curve expo strength AP_Float throttle_expo; + // QTILT mode forward throttle controls + AP_Int8 fwd_thr_chan; + AP_Float fwd_thr_mix; + RC_Channel *rc_fwd_thr_ch; + // QACRO mode max roll/pitch/yaw rates AP_Float acro_roll_rate; AP_Float acro_pitch_rate; diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 8f4df73dfb1b5..e436f734a43c5 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -267,6 +267,8 @@ void Plane::control_failsafe() case Mode::Number::QLAND: // throttle is ignored, but reset anyways case Mode::Number::QRTL: // throttle is ignored, but reset anyways case Mode::Number::QACRO: + case Mode::Number::QACRO_FTHR: + case Mode::Number::QSTAB_FTHR: case Mode::Number::QAUTOTUNE: if (quadplane.available() && quadplane.motors->get_desired_spool_state() > AP_Motors::DesiredSpoolState::GROUND_IDLE) { // set half throttle to avoid descending at maximum rate, still has a slight descent due to throttle deadzone diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 78b86df091b77..d510dcdbc6772 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -468,7 +468,7 @@ void Plane::set_servos_controlled(void) } else if (quadplane.in_vtol_mode()) { // ask quadplane code for forward throttle SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, - constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle)); + constrain_int16(quadplane.forward_throttle_pct(false), min_throttle, max_throttle)); } #if SOARING_ENABLED == ENABLED diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 18bdf6969aac5..8779be592077f 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -90,29 +90,48 @@ void QuadPlane::tiltrotor_continuous_update(void) tilt.current_throttle = constrain_float(motors_throttle, tilt.current_throttle-max_change, tilt.current_throttle+max_change); - + /* we are in a VTOL mode. We need to work out how much tilt is - needed. There are 3 strategies we will use: + needed. There are 4 strategies we will use: + + 1a) In QAUTOTUNE mode the angle will be set to zero. + 1b) In QACRO, QSTABILIZE and QHOVER modes with rc_fwd_thr_ch null, the angle will be set to zero. - 1) in QSTABILIZE or QHOVER the angle will be set to zero. This - enables these modes to be used as a safe recovery mode. + 2) In modes with manual forward throttle control - 2) in fixed wing assisted flight or velocity controlled modes we + 3) In fixed wing assisted flight or velocity controlled modes we will set the angle based on the demanded forward throttle, with a maximum tilt given by Q_TILT_MAX. This relies on - Q_VFWD_GAIN being set + Q_VFWD_GAIN being set. - 3) if we are in TRANSITION_TIMER mode then we are transitioning + 4) if we are in TRANSITION_TIMER mode then we are transitioning to forward flight and should put the rotors all the way forward */ - if (plane.control_mode == &plane.mode_qstabilize || - plane.control_mode == &plane.mode_qhover || - plane.control_mode == &plane.mode_qautotune) { + + if (plane.control_mode == &plane.mode_qautotune) { + // case 1a tiltrotor_slew(0); return; } + // if not transitioning to fixed wing + if (!assisted_flight) { + // case 1b) QACRO, QSTABILIZE or QHOVER mode + if (plane.control_mode == &plane.mode_qacro || + plane.control_mode == &plane.mode_qstabilize || + plane.control_mode == &plane.mode_qhover) { + tiltrotor_slew(0); + } + // case 2) manual control of forward throttle + if (plane.control_mode == &plane.mode_qafthr || + plane.control_mode == &plane.mode_qsfthr) { + float settilt = .01f * forward_throttle_pct(true); + tiltrotor_slew(settilt); + } + return; + } + if (assisted_flight && transition_state >= TRANSITION_TIMER) { // we are transitioning to fixed wing - tilt the motors all @@ -138,6 +157,7 @@ void QuadPlane::tiltrotor_binary_slew(bool forward) SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, forward?1000:0); // rate limiting current_tilt has the effect of delaying throttle in tiltrotor_binary_update + // The rate used here is (tilt.max_rate_down_dps > 0) ? tilt.max_rate_down_dps, tilt.max_rate_up_dps float max_change = tilt_max_change(!forward); if (forward) { tilt.current_tilt = constrain_float(tilt.current_tilt+max_change, 0, 1); From d67415665ceefeb1d90c6f2ccbfd45a908c2c05c Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 20 Nov 2019 17:34:49 -0700 Subject: [PATCH 3/4] Plane: use larger roll limit in mode QSFT --- ArduPlane/mode_qsfthr.cpp | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/ArduPlane/mode_qsfthr.cpp b/ArduPlane/mode_qsfthr.cpp index 8925f3e5b3b16..8e2335bd7890a 100644 --- a/ArduPlane/mode_qsfthr.cpp +++ b/ArduPlane/mode_qsfthr.cpp @@ -8,5 +8,32 @@ bool ModeQSFTHR::_enter() void ModeQSFTHR::update() { - return plane.mode_qstabilize.update(); + // set nav_roll and nav_pitch using sticks + + // use larger of Plane and quadplane roll limits + int16_t roll_limit = MAX(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); + + float pitch_input = plane.channel_pitch->norm_input(); + // Scale from normalized input [-1,1] to centidegrees + if (plane.quadplane.tailsitter_active()) { + // separate limit for tailsitter roll, if set + if (plane.quadplane.tailsitter.max_roll_angle > 0) { + roll_limit = plane.quadplane.tailsitter.max_roll_angle * 100.0f; + } + + // angle max for tailsitter pitch + plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max; + } else { + // pitch is further constrained by LIM_PITCH_MIN/MAX which may impose + // tighter (possibly asymmetrical) limits than Q_ANGLE_MAX + if (pitch_input > 0) { + plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max_cd, plane.quadplane.aparm.angle_max); + } else { + plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min_cd, plane.quadplane.aparm.angle_max); + } + plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + } + + plane.nav_roll_cd = (plane.channel_roll->get_control_in() / 4500.0) * roll_limit; + plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -roll_limit, roll_limit); } From 021893c253403ceb5647ffe0d5b1c6a31d2476a6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 4 Jan 2020 14:42:40 -0700 Subject: [PATCH 4/4] Plane: add TODO for mode_qsfthr.cpp --- ArduPlane/mode_qsfthr.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduPlane/mode_qsfthr.cpp b/ArduPlane/mode_qsfthr.cpp index 8e2335bd7890a..6b3ba4a74985e 100644 --- a/ArduPlane/mode_qsfthr.cpp +++ b/ArduPlane/mode_qsfthr.cpp @@ -6,11 +6,15 @@ bool ModeQSFTHR::_enter() return plane.mode_qstabilize._enter(); } +// this update method differs from ModeQStabilize::update() +// only in the roll_limit setting; it is higher since this is a more aerobatic +// use case than qstabilize +// TODO: add an argument to ModeQStabilize::update() instead of duplicating code void ModeQSFTHR::update() { // set nav_roll and nav_pitch using sticks - // use larger of Plane and quadplane roll limits + // use larger of Plane and quadplane roll limits (or just the plane limit) int16_t roll_limit = MAX(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); float pitch_input = plane.channel_pitch->norm_input();