From 1b5ed1caff5ebfedec0943ba4d445393b1389a2d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 27 Jul 2021 20:51:19 +0100 Subject: [PATCH] Copter: add turtle mode add turtle mode as an RC function add turtle mode arming checks re-use motortest arming checks for turtle mode --- ArduCopter/Copter.h | 6 +- ArduCopter/RC_Channel.cpp | 6 ++ ArduCopter/config.h | 6 ++ ArduCopter/mode.cpp | 6 ++ ArduCopter/mode.h | 25 ++++++ ArduCopter/mode_turtle.cpp | 165 +++++++++++++++++++++++++++++++++++++ ArduCopter/motor_test.cpp | 12 +-- 7 files changed, 219 insertions(+), 7 deletions(-) create mode 100644 ArduCopter/mode_turtle.cpp diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 6b8222a197..bc12255216 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -220,6 +220,7 @@ public: friend class ModeThrow; friend class ModeZigZag; friend class ModeAutorotate; + friend class ModeTurtle; Copter(void); @@ -816,7 +817,7 @@ private: // motor_test.cpp void motor_test_output(); - bool mavlink_motor_test_check(const GCS_MAVLINK &gcs_chan, bool check_rc); + bool mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check_rc, const char* mode); MAV_RESULT mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t motor_seq, uint8_t throttle_type, float throttle_value, float timeout_sec, uint8_t motor_count); void motor_test_stop(); @@ -982,6 +983,9 @@ private: #if MODE_AUTOROTATE_ENABLED == ENABLED ModeAutorotate mode_autorotate; #endif +#if MODE_TURTLE_ENABLED == ENABLED + ModeTurtle mode_turtle; +#endif // mode.cpp Mode *mode_from_mode_num(const Mode::Number mode); diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 01b1915952..b525bcc185 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -89,6 +89,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const AuxS case AUX_FUNC::ZIGZAG_SaveWP: case AUX_FUNC::ACRO: case AUX_FUNC::AUTO_RTL: + case AUX_FUNC::TURTLE: break; case AUX_FUNC::ACRO_TRAINER: case AUX_FUNC::ATTCON_ACCEL_LIM: @@ -568,6 +569,11 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi #endif break; + case AUX_FUNC::TURTLE: +#if MODE_TURTLE_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::TURTLE, ch_flag); +#endif + break; default: return RC_Channel::do_aux_function(ch_option, ch_flag); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 757de1c8d1..0fd289a7ef 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -335,6 +335,12 @@ # define MODE_ZIGZAG_ENABLED !HAL_MINIMIZE_FEATURES #endif +////////////////////////////////////////////////////////////////////////////// +// Turtle - allow vehicle to be flipped over after a crash +#ifndef MODE_TURTLE_ENABLED +# define MODE_TURTLE_ENABLED !HAL_MINIMIZE_FEATURES && !defined(DISABLE_DSHOT) && FRAME_CONFIG != HELI_FRAME +#endif + ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 87d46c4996..6263a6dd04 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -169,6 +169,12 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode) break; #endif +#if MODE_TURTLE_ENABLED == ENABLED + case Mode::Number::TURTLE: + ret = &mode_turtle; + break; +#endif + default: break; } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 05f209d247..43c743c454 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -37,6 +37,7 @@ public: SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers AUTOROTATE = 26, // Autonomous autorotation AUTO_RTL = 27, // Auto RTL, this is not a true mode, AUTO will report as this mode if entered to perform a DO_LAND_START Landing sequence + TURTLE = 28, // Flip over after crash }; // constructor @@ -1495,6 +1496,30 @@ private: float free_fall_start_velz; // vertical velocity when free fall was detected }; +#if MODE_TURTLE_ENABLED == ENABLED +class ModeTurtle : public Mode { + +public: + // inherit constructors + using Mode::Mode; + Number mode_number() const override { return Number::TURTLE; } + + bool init(bool ignore_checks) override; + void run() override; + void exit() override; + + bool requires_GPS() const override { return false; } + bool has_manual_throttle() const override { return true; } + bool allows_arming(AP_Arming::Method method) const override; + bool is_autopilot() const override { return false; } + void change_motor_direction(bool reverse); + +protected: + const char *name() const override { return "TURTLE"; } + const char *name4() const override { return "TRTL"; } +}; +#endif + // modes below rely on Guided mode so must be declared at the end (instead of in alphabetical order) class ModeAvoidADSB : public ModeGuided { diff --git a/ArduCopter/mode_turtle.cpp b/ArduCopter/mode_turtle.cpp new file mode 100644 index 0000000000..6592fd11c0 --- /dev/null +++ b/ArduCopter/mode_turtle.cpp @@ -0,0 +1,165 @@ +#include "Copter.h" + +#if MODE_TURTLE_ENABLED == ENABLED + +#define CRASH_FLIP_EXPO 35.0f +#define CRASH_FLIP_STICK_MINF 0.15f +#define power3(x) ((x)*(x)*(x)) + +bool ModeTurtle::init(bool ignore_checks) +{ + // do not enter the mode when already armed or when flying + if (motors->armed() || SRV_Channels::get_dshot_esc_type() == 0) { + return false; + } + + // perform minimal arming checks + if (!copter.mavlink_motor_control_check(*gcs().chan(0), true, "Turtle Mode")) { + return false; + } + + // do not enter the mode if sticks are not centered + if (!is_zero(channel_pitch->norm_input_dz()) + || !is_zero(channel_roll->norm_input_dz()) + || !is_zero(channel_yaw->norm_input_dz())) { + return false; + } + // reverse the motors + hal.rcout->disable_channel_mask_updates(); + change_motor_direction(true); + + // disable throttle and gps failsafe + g.failsafe_throttle = FS_THR_DISABLED; + g.failsafe_gcs = FS_GCS_DISABLED; + g.fs_ekf_action = 0; + + // arm + motors->armed(true); + hal.util->set_soft_armed(true); + + return true; +} + +bool ModeTurtle::allows_arming(AP_Arming::Method method) const +{ + return true; +} + +void ModeTurtle::exit() +{ + // disarm + motors->armed(false); + hal.util->set_soft_armed(false); + + // un-reverse the motors + change_motor_direction(false); + hal.rcout->enable_channel_mask_updates(); + + // re-enable failsafes + g.failsafe_throttle.load(); + g.failsafe_gcs.load(); + g.fs_ekf_action.load(); +} + +void ModeTurtle::change_motor_direction(bool reverse) +{ + AP_HAL::RCOutput::BLHeliDshotCommand direction = reverse ? AP_HAL::RCOutput::DSHOT_REVERSE : AP_HAL::RCOutput::DSHOT_NORMAL; + AP_HAL::RCOutput::BLHeliDshotCommand inverse_direction = reverse ? AP_HAL::RCOutput::DSHOT_NORMAL : AP_HAL::RCOutput::DSHOT_REVERSE; + + if (!hal.rcout->get_reversed_mask()) { + hal.rcout->send_dshot_command(direction, AP_HAL::RCOutput::ALL_CHANNELS, 0, 10, true); + } else { + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) { + if (!motors->is_motor_enabled(i)) { + continue; + } + + if ((hal.rcout->get_reversed_mask() & (1U<send_dshot_command(direction, i, 0, 10, true); + } else { + hal.rcout->send_dshot_command(inverse_direction, i, 0, 10, true); + } + } + } +} + +void ModeTurtle::run() +{ + const float flipPowerFactor = 1.0f - CRASH_FLIP_EXPO / 100.0f; + const bool norc = copter.failsafe.radio || !copter.ap.rc_receiver_present; + const float stickDeflectionPitch = norc ? 0.0f : channel_pitch->norm_input_dz(); + const float stickDeflectionRoll = norc ? 0.0f : channel_roll->norm_input_dz(); + const float stickDeflectionYaw = norc ? 0.0f : channel_yaw->norm_input_dz(); + + const float stickDeflectionPitchAbs = fabsf(stickDeflectionPitch); + const float stickDeflectionRollAbs = fabsf(stickDeflectionRoll); + const float stickDeflectionYawAbs = fabsf(stickDeflectionYaw); + + const float stickDeflectionPitchExpo = flipPowerFactor * stickDeflectionPitchAbs + power3(stickDeflectionPitchAbs) * (1 - flipPowerFactor); + const float stickDeflectionRollExpo = flipPowerFactor * stickDeflectionRollAbs + power3(stickDeflectionRollAbs) * (1 - flipPowerFactor); + const float stickDeflectionYawExpo = flipPowerFactor * stickDeflectionYawAbs + power3(stickDeflectionYawAbs) * (1 - flipPowerFactor); + + float signPitch = stickDeflectionPitch < 0 ? -1 : 1; + float signRoll = stickDeflectionRoll < 0 ? 1 : -1; + //float signYaw = stickDeflectionYaw < 0 ? -1 : 1; + + float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs)); + float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo)); + + if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) { + // If yaw is the dominant, disable pitch and roll + stickDeflectionLength = stickDeflectionYawAbs; + stickDeflectionExpoLength = stickDeflectionYawExpo; + signRoll = 0; + signPitch = 0; + } else { + // If pitch/roll dominant, disable yaw + //signYaw = 0; + } + + const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength) : 0; + const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f) + + if (cosPhi < cosThreshold) { + // Enforce either roll or pitch exclusively, if not on diagonal + if (stickDeflectionRollAbs > stickDeflectionPitchAbs) { + signPitch = 0; + } else { + signRoll = 0; + } + } + + // Apply a reasonable amount of stick deadband + const float crashFlipStickMinExpo = flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor); + const float flipStickRange = 1.0f - crashFlipStickMinExpo; + const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange; + + // at this point we have a power value in the range 0..1 + + // notmalise the roll and pitch input to match the motors + Vector2f input {signRoll, signPitch}; + input = input.normalized() * 0.5; + + // we bypass spin min and friends in the deadzone because we only want spin up when the sticks are moved + float motorOutput = !is_zero(flipPower) ? motors->thrust_to_actuator(flipPower) : 0.0f; + + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) { + if (!motors->is_motor_enabled(i)) { + continue; + } + + const Vector2f output {motors->get_roll_factor(i), motors->get_pitch_factor(i)}; + // if output aligns with input then use this motor + if ((input - output).length() > 0.5) { + motors->rc_write(i, motors->get_pwm_output_min()); + continue; + } + + int16_t pwm = motors->get_pwm_output_min() + + (motors->get_pwm_output_max() - motors->get_pwm_output_min()) * motorOutput; + + motors->rc_write(i, pwm); + } +} + +#endif diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 2ac911f8f3..d684f86e02 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -95,29 +95,29 @@ void Copter::motor_test_output() // mavlink_motor_test_check - perform checks before motor tests can begin // return true if tests can continue, false if not -bool Copter::mavlink_motor_test_check(const GCS_MAVLINK &gcs_chan, bool check_rc) +bool Copter::mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check_rc, const char* mode) { // check board has initialised if (!ap.initialised) { - gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Board initialising"); + gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: Board initialising", mode); return false; } // check rc has been calibrated if (check_rc && !arming.rc_calibration_checks(true)) { - gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated"); + gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: RC not calibrated", mode); return false; } // ensure we are landed if (!ap.land_complete) { - gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: vehicle not landed"); + gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: vehicle not landed", mode); return false; } // check if safety switch has been pushed if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { - gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Safety switch"); + gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: Safety switch", mode); return false; } @@ -141,7 +141,7 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t The RC calibrated check can be skipped if direct pwm is supplied */ - if (!mavlink_motor_test_check(gcs_chan, throttle_type != 1)) { + if (!mavlink_motor_control_check(gcs_chan, throttle_type != 1, "Motor Test")) { return MAV_RESULT_FAILED; } else { // start test