From d949c80d5490d894b0d275f8b8bd5dc33db21f18 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Thu, 28 Feb 2019 18:03:00 +0900 Subject: [PATCH] Copter: tradheli replaces rotor_runup_complete with spool state --- ArduCopter/Copter.h | 3 +- ArduCopter/heli.cpp | 2 +- ArduCopter/land_detector.cpp | 2 +- ArduCopter/mode.cpp | 9 +++--- ArduCopter/mode_acro_heli.cpp | 38 +++++++++++++++++-------- ArduCopter/mode_brake.cpp | 7 ++++- ArduCopter/mode_circle.cpp | 7 ++++- ArduCopter/mode_flowhold.cpp | 7 ----- ArduCopter/mode_guided.cpp | 4 +-- ArduCopter/mode_stabilize_heli.cpp | 45 ++++++++++++++++++++---------- ArduCopter/motors.cpp | 4 +-- ArduCopter/system.cpp | 20 ++++++------- ArduCopter/takeoff.cpp | 4 +-- 13 files changed, 88 insertions(+), 64 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index fbd01da78a..c8c7d43e71 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -538,8 +538,7 @@ private: // Tradheli flags typedef struct { uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms) - uint8_t init_targets_on_arming : 1; // 1 // true if we have been disarmed, and need to reset rate controller targets when we arm - uint8_t inverted_flight : 1; // 2 // true for inverted flight mode + uint8_t inverted_flight : 1; // 1 // true for inverted flight mode } heli_flags_t; heli_flags_t heli_flags; diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index fb09c8db16..7bc927dd95 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -23,7 +23,7 @@ void Copter::heli_init() // should be called at 50hz void Copter::check_dynamic_flight(void) { - if (!motors->armed() || !motors->rotor_runup_complete() || + if (motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED || control_mode == LAND || (control_mode==RTL && mode_rtl.state() == RTL_Land) || (control_mode == AUTO && mode_auto.mode() == Auto_Land)) { heli_dynamic_flight_counter = 0; heli_flags.dynamic_flight = false; diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index a51ee718bc..f6272912b7 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -47,7 +47,7 @@ void Copter::update_land_detector() } else if (ap.land_complete) { #if FRAME_CONFIG == HELI_FRAME // if rotor speed and collective pitch are high then clear landing flag - if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->rotor_runup_complete()) { + if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { #else // if throttle output is high then clear landing flag if (motors->get_throttle() > get_non_takeoff_throttle()) { diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index be28d36549..d7732b37f6 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -193,7 +193,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) #if FRAME_CONFIG == HELI_FRAME // do not allow helis to enter a non-manual throttle mode if the // rotor runup is not complete - if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()){ + if (!ignore_checks && !new_flightmode->has_manual_throttle() && (motors->get_spool_mode() == AP_Motors::SPOOL_UP || motors->get_spool_mode() == AP_Motors::SPOOL_DOWN)) { gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed"); AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; @@ -381,12 +381,11 @@ bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const // can't takeoff unless we want to go up... return false; } -#if FRAME_CONFIG == HELI_FRAME - if (!copter.motors->rotor_runup_complete()) { - // hold heli on the ground until rotor speed runup has finished + if (copter.motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED) { + // hold aircraft on the ground until rotor speed runup has finished return false; } -#endif + return true; } diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index 94461acf71..1bf54c5c59 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -29,20 +29,34 @@ void Copter::ModeAcro_Heli::run() float target_roll, target_pitch, target_yaw; float pilot_throttle_scaled; - // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because - // we may be in autorotation flight. These should be reset only when transitioning from disarmed - // to armed, because the pilot will have placed the helicopter down on the landing pad. This is so - // that the servos move in a realistic fashion while disarmed for operational checks. - // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move - - if (motors->init_targets_on_arming()) { - attitude_control->reset_rate_controller_I_terms(); - attitude_control->set_attitude_target_to_current_attitude(); + // Tradheli should not reset roll, pitch, yaw targets when motors are not runup while flying, because + // we may be in autorotation flight. This is so that the servos move in a realistic fashion while disarmed + // for operational checks. Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero + // so the swash servos move. + + if (!motors->armed()) { + // Motors should be Stopped + motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + } else { + // heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } - // clear landing flag above zero throttle - if (motors->armed() && motors->get_interlock() && motors->rotor_runup_complete() && !ap.throttle_zero) { - set_land_complete(false); + if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) { + // Motors Stopped + attitude_control->set_attitude_target_to_current_attitude(); + attitude_control->reset_rate_controller_I_terms(); + } else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + // Landed + if (motors->init_targets_on_arming()) { + attitude_control->set_attitude_target_to_current_attitude(); + attitude_control->reset_rate_controller_I_terms(); + } + } else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + // clear landing flag above zero throttle + if (!motors->limit.throttle_lower) { + set_land_complete(false); + } } motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 0e42277d6b..6b2bca08c4 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -61,7 +61,12 @@ void Copter::ModeBrake::run() // body-frame rate controller is run directly from 100hz loop // update altitude target and call position controller - pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); + // protects heli's from inflight motor interlock disable + if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) { + pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); + } else { + pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); + } pos_control->update_z_controller(); if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) { diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 6a13329bbc..b262e7d973 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -85,7 +85,12 @@ void Copter::ModeCircle::run() target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); // update altitude target and call position controller - pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); + // protects heli's from inflight motor interlock disable + if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) { + pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); + } else { + pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); + } pos_control->update_z_controller(); } diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 42f6942333..7d212a13c3 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -84,13 +84,6 @@ Copter::ModeFlowHold::ModeFlowHold(void) : Mode() // flowhold_init - initialise flowhold controller bool Copter::ModeFlowHold::init(bool ignore_checks) { -#if FRAME_CONFIG == HELI_FRAME - // do not allow helis to enter Flow Hold if the Rotor Runup is not complete - if (!ignore_checks && !motors->rotor_runup_complete()){ - return false; - } -#endif - if (!copter.optflow.enabled() || !copter.optflow.healthy()) { return false; } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index a8cfc42b13..f4f6c56304 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -395,8 +395,8 @@ void Copter::Mode::auto_takeoff_run() } #if FRAME_CONFIG == HELI_FRAME - // helicopters stay in landed state until rotor speed runup has finished - if (motors->rotor_runup_complete()) { + // aircraft stays in landed state until rotor speed runup has finished + if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { set_land_complete(false); } else { // initialise wpnav targets diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index 696ad9e3ac..35054e4985 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/ArduCopter/mode_stabilize_heli.cpp @@ -22,22 +22,7 @@ void Copter::ModeStabilize_Heli::run() float target_yaw_rate; float pilot_throttle_scaled; - // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because - // we may be in autorotation flight. These should be reset only when transitioning from disarmed - // to armed, because the pilot will have placed the helicopter down on the landing pad. This is so - // that the servos move in a realistic fashion while disarmed for operational checks. - // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move - if (motors->init_targets_on_arming()) { - attitude_control->reset_rate_controller_I_terms(); - attitude_control->set_yaw_target_to_current_heading(); - } - - // clear landing flag above zero throttle - if (motors->armed() && motors->get_interlock() && motors->rotor_runup_complete() && !ap.throttle_zero) { - set_land_complete(false); - } - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // apply SIMPLE mode transform to pilot inputs @@ -52,6 +37,36 @@ void Copter::ModeStabilize_Heli::run() // get pilot's desired throttle pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in()); + // Tradheli should not reset roll, pitch, yaw targets when motors are not runup while flying, because + // we may be in autorotation flight. This is so that the servos move in a realistic fashion while disarmed + // for operational checks. Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero + // so the swash servos move. + + if (!motors->armed()) { + // Motors should be Stopped + motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + } else { + // heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + } + + if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) { + // Motors Stopped + attitude_control->set_yaw_target_to_current_heading(); + attitude_control->reset_rate_controller_I_terms(); + } else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + // Landed + if (motors->init_targets_on_arming()) { + attitude_control->set_yaw_target_to_current_heading(); + attitude_control->reset_rate_controller_I_terms(); + } + } else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + // clear landing flag above zero throttle + if (!motors->limit.throttle_lower) { + set_land_complete(false); + } + } + // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index fa53090a95..e9b15a0ec5 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -93,13 +93,11 @@ void Copter::auto_disarm_check() return; } -#if FRAME_CONFIG == HELI_FRAME // if the rotor is still spinning, don't initiate auto disarm - if (motors->rotor_speed_above_critical()) { + if (motors->get_spool_mode() != AP_Motors::GROUND_IDLE) { auto_disarm_begin = tnow_ms; return; } -#endif // always allow auto disarm if using interlock switch or motors are Emergency Stopped if ((ap.using_interlock && !motors->get_interlock()) || SRV_Channels::get_emergency_stop()) { diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 980df42206..9853bfc454 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -368,28 +368,26 @@ void Copter::update_auto_armed() if(flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio) { set_auto_armed(false); } -#if FRAME_CONFIG == HELI_FRAME // 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->rotor_runup_complete()) { + if(ap.land_complete && motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED && ap.using_interlock) { set_auto_armed(false); } -#endif // HELI_FRAME }else{ // arm checks -#if FRAME_CONFIG == HELI_FRAME // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true - if(motors->armed() && !ap.throttle_zero && motors->rotor_runup_complete()) { - set_auto_armed(true); - } -#else + if(motors->armed() && ap.using_interlock) { + if(!ap.throttle_zero && motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + set_auto_armed(true); + } // if motors are armed and throttle is above zero auto_armed should be true // if motors are armed and we are in throw mode, then auto_armed should be true - if(motors->armed() && (!ap.throttle_zero || control_mode == THROW)) { - set_auto_armed(true); + } else if (motors->armed() && !ap.using_interlock) { + if(!ap.throttle_zero || control_mode == THROW) { + set_auto_armed(true); + } } -#endif // HELI_FRAME } } diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 3e122ce6a8..cad132b906 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -32,12 +32,10 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) return false; } -#if FRAME_CONFIG == HELI_FRAME // Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning - if (!copter.motors->rotor_runup_complete()) { + if (motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED && ap.using_interlock) { return false; } -#endif if (!do_user_takeoff_start(takeoff_alt_cm)) { return false;