diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5718fc11f4..9915885060 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -390,7 +390,6 @@ private: // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, Mode *flightmode; - Mode::Number prev_control_mode; RCMapper rcmap; diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 7b4870de5a..bff2a3de17 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -189,29 +189,25 @@ void Copter::heli_update_rotor_speed_targets() // to autorotation flight mode if manual collective is not being used. void Copter::heli_update_autorotation() { - // check if flying and interlock disengaged - if (!ap.land_complete && !motors->get_interlock()) { + bool in_autorotation_mode = false; #if MODE_AUTOROTATE_ENABLED - if (g2.arot.is_enable()) { - if (!flightmode->has_manual_throttle()) { - // set autonomous autorotation flight mode - set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START); - } - // set flag to facilitate both auto and manual autorotations - motors->set_in_autorotation(true); - motors->set_enable_bailout(true); - } + in_autorotation_mode = flightmode == &mode_autorotate; #endif - if (flightmode->has_manual_throttle() && motors->arot_man_enabled()) { - // set flag to facilitate both auto and manual autorotations - motors->set_in_autorotation(true); - motors->set_enable_bailout(true); - } - } else { - motors->set_in_autorotation(false); - motors->set_enable_bailout(false); + + // If we have landed then we do not want to be in autorotation and we do not want to via the bailout state + if (ap.land_complete || ap.land_complete_maybe) { + motors->force_deactivate_autorotation(); + return; } + // if we got this far we are flying, check for conditions to set autorotation state + if (!motors->get_interlock() && (flightmode->has_manual_throttle() || in_autorotation_mode)) { + // set state in motors to facilitate manual and assisted autorotations + motors->set_autorotation_active(true); + } else { + // deactivate the autorotation state via the bailout case + motors->set_autorotation_active(false); + } } // update collective low flag. Use a debounce time of 400 milliseconds. diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 74e3cbb3c0..5d8fc5a9e4 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -292,20 +292,9 @@ bool Copter::set_mode(Mode::Number mode, ModeReason 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->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) { - #if MODE_AUTOROTATE_ENABLED - //if the mode being exited is the autorotation mode allow mode change despite rotor not being at - //full speed. This will reduce altitude loss on bail-outs back to non-manual throttle modes - bool in_autorotation_check = (flightmode != &mode_autorotate || new_flightmode != &mode_autorotate); - #else - bool in_autorotation_check = false; - #endif - - if (!in_autorotation_check) { - mode_change_failed(new_flightmode, "runup not complete"); - return false; - } + if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()) { + mode_change_failed(new_flightmode, "runup not complete"); + return false; } #endif @@ -369,9 +358,6 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) // perform any cleanup required by previous flight mode exit_mode(flightmode, new_flightmode); - // store previous flight mode (only used by tradeheli's autorotation) - prev_control_mode = flightmode->mode_number(); - // update flight mode flightmode = new_flightmode; control_mode_reason = reason; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 98f01449c5..faf22b0180 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -2015,18 +2015,14 @@ private: int32_t _pitch_target; // Target pitch attitude to pass to attitude controller uint32_t _entry_time_start_ms; // Time remaining until entry phase moves on to glide phase float _hs_decay; // The head accerleration during the entry phase - float _bail_time; // Timer for exiting the bail out phase (s) - uint32_t _bail_time_start_ms; // Time at start of bail out - float _target_climb_rate_adjust;// Target vertical acceleration used during bail out phase - float _target_pitch_adjust; // Target pitch rate used during bail out phase enum class Autorotation_Phase { ENTRY, SS_GLIDE, FLARE, TOUCH_DOWN, - BAIL_OUT } phase_switch; - + LANDED } phase_switch; + enum class Navigation_Decision { USER_CONTROL_STABILISED, STRAIGHT_AHEAD, @@ -2039,10 +2035,10 @@ private: bool ss_glide_initial : 1; bool flare_initial : 1; bool touch_down_initial : 1; + bool landed_initial : 1; bool straight_ahead_initial : 1; bool level_initial : 1; bool break_initial : 1; - bool bail_out_initial : 1; bool bad_rpm : 1; } _flags; diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index cc97d75f30..e13bc52516 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -14,8 +14,8 @@ #if MODE_AUTOROTATE_ENABLED #define AUTOROTATE_ENTRY_TIME 2.0f // (s) number of seconds that the entry phase operates for -#define BAILOUT_MOTOR_RAMP_TIME 1.0f // (s) time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single #define HEAD_SPEED_TARGET_RATIO 1.0f // Normalised target main rotor head speed (unit: -) +#define AUTOROTATION_MIN_MOVING_SPEED 100.0 // (cm/s) minimum speed used for "is moving" check bool ModeAutorotate::init(bool ignore_checks) { @@ -24,15 +24,16 @@ bool ModeAutorotate::init(bool ignore_checks) return false; #endif - // Check that mode is enabled + // Check that mode is enabled, make sure this is the first check as this is the most + // important thing for users to fix if they are planning to use autorotation mode if (!g2.arot.is_enable()) { - gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Not Enabled"); + gcs().send_text(MAV_SEVERITY_WARNING, "Autorot Mode Not Enabled"); return false; } - // Check that interlock is disengaged - if (motors->get_interlock()) { - gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Change Fail: Interlock Engaged"); + // Must be armed to use mode, prevent triggering state machine on the ground + if (!motors->armed() || copter.ap.land_complete || copter.ap.land_complete_maybe) { + gcs().send_text(MAV_SEVERITY_WARNING, "Autorot: Must be Armed and Flying"); return false; } @@ -52,10 +53,10 @@ bool ModeAutorotate::init(bool ignore_checks) _flags.ss_glide_initial = true; _flags.flare_initial = true; _flags.touch_down_initial = true; + _flags.landed_initial = true; _flags.level_initial = true; _flags.break_initial = true; _flags.straight_ahead_initial = true; - _flags.bail_out_initial = true; _msg_flags.bad_rpm = true; // Setting default starting switches @@ -74,20 +75,9 @@ bool ModeAutorotate::init(bool ignore_checks) void ModeAutorotate::run() { - // Check if interlock becomes engaged - if (motors->get_interlock() && !copter.ap.land_complete) { - phase_switch = Autorotation_Phase::BAIL_OUT; - } else if (motors->get_interlock() && copter.ap.land_complete) { - // Aircraft is landed and no need to bail out - set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT); - } - // Current time uint32_t now = millis(); //milliseconds - // Initialise internal variables - float curr_vel_z = inertial_nav.get_velocity_z_up_cms(); // Current vertical descent - //---------------------------------------------------------------- // State machine logic //---------------------------------------------------------------- @@ -97,12 +87,22 @@ void ModeAutorotate::run() // Timer from entry phase to progress to glide phase if (phase_switch == Autorotation_Phase::ENTRY){ - if ((now - _entry_time_start_ms)/1000.0f > AUTOROTATE_ENTRY_TIME) { // Flight phase can be progressed to steady state glide phase_switch = Autorotation_Phase::SS_GLIDE; } + } + // Check if we believe we have landed. We need the landed state to zero all controls and make sure that the copter landing detector will trip + bool speed_check = inertial_nav.get_velocity_z_up_cms() < AUTOROTATION_MIN_MOVING_SPEED && + inertial_nav.get_speed_xy_cms() < AUTOROTATION_MIN_MOVING_SPEED; + if (motors->get_below_land_min_coll() && AP::ins().is_still() && speed_check) { + phase_switch = Autorotation_Phase::LANDED; + } + + // Check if we are bailing out and need to re-set the spool state + if (motors->autorotation_bailout()) { + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } @@ -199,79 +199,22 @@ void ModeAutorotate::run() { break; } - - case Autorotation_Phase::BAIL_OUT: + case Autorotation_Phase::LANDED: { - if (_flags.bail_out_initial == true) { - // Functions and settings to be done once are done here. + // Entry phase functions to be run only once + if (_flags.landed_initial == true) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation"); + gcs().send_text(MAV_SEVERITY_INFO, "Landed"); #endif - - // Set bail out timer remaining equal to the parameter value, bailout time - // cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME. - _bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f); - - // Set bail out start time - _bail_time_start_ms = now; - - // Set initial target vertical speed - _desired_v_z = curr_vel_z; - - // Initialise position and desired velocity - if (!pos_control->is_active_z()) { - pos_control->relax_z_controller(g2.arot.get_last_collective()); - } - - // Get pilot parameter limits - const float pilot_spd_dn = -get_pilot_speed_dn(); - const float pilot_spd_up = g.pilot_speed_up; - - float pilot_des_v_z = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - pilot_des_v_z = constrain_float(pilot_des_v_z, pilot_spd_dn, pilot_spd_up); - - // Calculate target climb rate adjustment to transition from bail out descent speed to requested climb rate on stick. - _target_climb_rate_adjust = (curr_vel_z - pilot_des_v_z)/(_bail_time - BAILOUT_MOTOR_RAMP_TIME); //accounting for 0.5s motor spool time - - // Calculate pitch target adjustment rate to return to level - _target_pitch_adjust = _pitch_target/_bail_time; - - // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); - pos_control->set_correction_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); - - motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - _flags.bail_out_initial = false; + _flags.landed_initial = false; } - - if ((now - _bail_time_start_ms)/1000.0f >= BAILOUT_MOTOR_RAMP_TIME) { - // Update desired vertical speed and pitch target after the bailout motor ramp timer has completed - _desired_v_z -= _target_climb_rate_adjust*G_Dt; - _pitch_target -= _target_pitch_adjust*G_Dt; - } - // Set position controller - pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z); - - // Update controllers - pos_control->update_z_controller(); - - if ((now - _bail_time_start_ms)/1000.0f >= _bail_time) { - // Bail out timer complete. Change flight mode. Do not revert back to auto. Prevent aircraft - // from continuing mission and potentially flying further away after a power failure. - if (copter.prev_control_mode == Mode::Number::AUTO) { - set_mode(Mode::Number::ALT_HOLD, ModeReason::AUTOROTATION_BAILOUT); - } else { - set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT); - } - } - - break; + // don't allow controller to continually ask for more pitch to build speed when we are on the ground, decay to zero smoothly + _pitch_target *= 0.95; + break; } } - switch (nav_pos_switch) { case Navigation_Decision::USER_CONTROL_STABILISED: diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 3b5028ad4b..f44a7bca2d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -540,7 +540,7 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang update_althold_lean_angle_max(throttle_in); _motors.set_throttle_filter_cutoff(filter_cutoff); - if (apply_angle_boost && !((AP_MotorsHeli&)_motors).get_in_autorotation()) { + if (apply_angle_boost && !((AP_MotorsHeli&)_motors).in_autorotation()) { // Apply angle boost throttle_in = get_throttle_boosted(throttle_in); } else {