From ca007ee6f657f042772a08cef63188b206b11017 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Tue, 13 Oct 2020 19:19:42 -0400 Subject: [PATCH] Copter: Allow Tradheli to spoolup in guided or auto mode --- ArduCopter/mode.cpp | 19 ++++++++++++++++--- ArduCopter/mode.h | 2 +- ArduCopter/mode_auto.cpp | 6 +++--- ArduCopter/mode_brake.cpp | 2 +- ArduCopter/mode_circle.cpp | 2 +- ArduCopter/mode_follow.cpp | 2 +- ArduCopter/mode_guided.cpp | 15 ++++++++++----- ArduCopter/mode_land.cpp | 4 ++-- ArduCopter/mode_rtl.cpp | 8 ++++---- ArduCopter/system.cpp | 6 +----- ArduCopter/takeoff.cpp | 8 +++++--- 11 files changed, 45 insertions(+), 29 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 4c8ba3738b..a668503d97 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -477,10 +477,23 @@ void Mode::zero_throttle_and_hold_attitude() attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt); } -void Mode::make_safe_spool_down() +// handle situations where the vehicle is on the ground waiting for takeoff +// force_throttle_unlimited should be true in cases where we want to keep the motors spooled up +// (instead of spooling down to ground idle). This is required for tradheli's in Guided and Auto +// where we always want the motor spooled up in Guided or Auto mode. Tradheli's main rotor stops +// when spooled down to ground idle. +// ultimately it forces the motor interlock to be obeyed in auto and guided modes when on the ground. +void Mode::make_safe_ground_handling(bool force_throttle_unlimited) { - // command aircraft to initiate the shutdown process - motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); + if (force_throttle_unlimited) { + // keep rotors turning + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + } else { + // spool down to ground idle + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); + } + + switch (motors->get_spool_state()) { case AP_Motors::SpoolState::SHUT_DOWN: diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 43c743c454..773d0c908d 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -109,7 +109,7 @@ protected: bool is_disarmed_or_landed() const; void zero_throttle_and_relax_ac(bool spool_up = false); void zero_throttle_and_hold_attitude(); - void make_safe_spool_down(); + void make_safe_ground_handling(bool force_throttle_unlimited = false); // functions to control landing // in modes that support landing diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index e4f56cd147..6da5ff21d7 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -843,7 +843,7 @@ void ModeAuto::wp_run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); wp_nav->wp_and_spline_init(); return; } @@ -875,7 +875,7 @@ void ModeAuto::land_run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); return; @@ -942,7 +942,7 @@ void ModeAuto::loiter_run() { // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); wp_nav->wp_and_spline_init(); return; } diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index a11bec1aac..5065208409 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -36,7 +36,7 @@ void ModeBrake::run() { // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); pos_control->relax_velocity_controller_xy(); pos_control->relax_z_controller(0.0f); return; diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index f34b9263f1..43e2230893 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -97,7 +97,7 @@ void ModeCircle::run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); return; } diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 64519e8ea7..a83719d09d 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -34,7 +34,7 @@ void ModeFollow::run() { // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); return; } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 6762a58e79..1668cf5b79 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -535,7 +535,8 @@ void ModeGuided::pos_control_run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); return; } @@ -593,7 +594,8 @@ void ModeGuided::accel_control_run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); return; } @@ -656,7 +658,8 @@ void ModeGuided::velaccel_control_run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); return; } @@ -731,7 +734,8 @@ void ModeGuided::posvelaccel_control_run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); return; } @@ -845,7 +849,8 @@ void ModeGuided::angle_control_run() // if not armed set throttle to zero and exit immediately if (!motors->armed() || !copter.ap.auto_armed || (copter.ap.land_complete && !positive_thrust_or_climbrate)) { - make_safe_spool_down(); + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); return; } diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 9f656314b9..655effb5e5 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -66,7 +66,7 @@ void ModeLand::gps_run() // Land State Machine Determination if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); } else { @@ -121,7 +121,7 @@ void ModeLand::nogps_run() // Land State Machine Determination if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); } else { // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index c5e88ea7b9..3928a0e200 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -153,7 +153,7 @@ void ModeRTL::climb_return_run() { // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); return; } @@ -211,7 +211,7 @@ void ModeRTL::loiterathome_run() { // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); return; } @@ -294,7 +294,7 @@ void ModeRTL::descent_run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); return; } @@ -398,7 +398,7 @@ void ModeRTL::land_run(bool disarm_on_land) // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_spool_down(); + make_safe_ground_handling(); loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); return; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 93cb40eb00..8989c9c3d6 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -390,11 +390,7 @@ void Copter::update_auto_armed() if(flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio) { set_auto_armed(false); } - // if helicopters are on the ground, and the motor is switched off, auto-armed should be false - // so that rotor runup is checked again before attempting to take-off - if(ap.land_complete && motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && ap.using_interlock) { - set_auto_armed(false); - } + }else{ // arm checks diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index dc8c6b69e2..e79d5909fc 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -35,8 +35,9 @@ bool Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) return false; } - // Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning - if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && copter.ap.using_interlock) { + // Vehicles using motor interlock should return false if motor interlock is disabled. + // Interlock must be enabled to allow the controller to spool up the motor(s) for takeoff. + if (!motors->get_interlock() && copter.ap.using_interlock) { return false; } @@ -96,7 +97,8 @@ void Mode::auto_takeoff_run() { // if not armed set throttle to zero and exit immediately if (!motors->armed() || !copter.ap.auto_armed) { - make_safe_spool_down(); + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); wp_nav->shift_wp_origin_and_destination_to_current_pos_xy(); return; }