1414#if MODE_AUTOROTATE_ENABLED
1515
1616#define AUTOROTATE_ENTRY_TIME 2 .0f // (s) number of seconds that the entry phase operates for
17- #define BAILOUT_MOTOR_RAMP_TIME 1 .0f // (s) time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single
1817#define HEAD_SPEED_TARGET_RATIO 1 .0f // Normalised target main rotor head speed (unit: -)
18+ #define AUTOROTATION_MIN_MOVING_SPEED 100.0 // (cm/s) minimum speed used for "is moving" check
1919
2020bool ModeAutorotate::init (bool ignore_checks)
2121{
@@ -24,15 +24,16 @@ bool ModeAutorotate::init(bool ignore_checks)
2424 return false ;
2525#endif
2626
27- // Check that mode is enabled
27+ // Check that mode is enabled, make sure this is the first check as this is the most
28+ // important thing for users to fix if they are planning to use autorotation mode
2829 if (!g2.arot .is_enable ()) {
29- gcs ().send_text (MAV_SEVERITY_INFO , " Autorot Mode Not Enabled"
30+ gcs ().send_text (MAV_SEVERITY_WARNING , " Autorot Mode Not Enabled"
3031 return false ;
3132 }
3233
33- // Check that interlock is disengaged
34- if (motors->get_interlock () ) {
35- gcs ().send_text (MAV_SEVERITY_INFO , " Autorot Mode Change Fail: Interlock Engaged "
34+ // Must be armed to use mode, prevent triggering state machine on the ground
35+ if (! motors->armed () || copter. ap . land_complete || copter. ap . land_complete_maybe ) {
36+ gcs ().send_text (MAV_SEVERITY_WARNING , " Autorot: Must be Armed and Flying "
3637 return false ;
3738 }
3839
@@ -52,10 +53,10 @@ bool ModeAutorotate::init(bool ignore_checks)
5253 _flags.ss_glide_initial = true ;
5354 _flags.flare_initial = true ;
5455 _flags.touch_down_initial = true ;
56+ _flags.landed_initial = true ;
5557 _flags.level_initial = true ;
5658 _flags.break_initial = true ;
5759 _flags.straight_ahead_initial = true ;
58- _flags.bail_out_initial = true ;
5960 _msg_flags.bad_rpm = true ;
6061
6162 // Setting default starting switches
@@ -74,20 +75,9 @@ bool ModeAutorotate::init(bool ignore_checks)
7475
7576void ModeAutorotate::run ()
7677{
77- // Check if interlock becomes engaged
78- if (motors->get_interlock () && !copter.ap .land_complete ) {
79- phase_switch = Autorotation_Phase::BAIL_OUT;
80- } else if (motors->get_interlock () && copter.ap .land_complete ) {
81- // Aircraft is landed and no need to bail out
82- set_mode (copter.prev_control_mode , ModeReason::AUTOROTATION_BAILOUT);
83- }
84-
8578 // Current time
8679 uint32_t now = millis (); // milliseconds
8780
88- // Initialise internal variables
89- float curr_vel_z = inertial_nav.get_velocity_z_up_cms (); // Current vertical descent
90-
9181 // ----------------------------------------------------------------
9282 // State machine logic
9383 // ----------------------------------------------------------------
@@ -97,12 +87,22 @@ void ModeAutorotate::run()
9787
9888 // Timer from entry phase to progress to glide phase
9989 if (phase_switch == Autorotation_Phase::ENTRY){
100-
10190 if ((now - _entry_time_start_ms)/1000 .0f > AUTOROTATE_ENTRY_TIME) {
10291 // Flight phase can be progressed to steady state glide
10392 phase_switch = Autorotation_Phase::SS_GLIDE;
10493 }
94+ }
95+
96+ // 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
97+ bool speed_check = inertial_nav.get_velocity_z_up_cms () < AUTOROTATION_MIN_MOVING_SPEED &&
98+ inertial_nav.get_speed_xy_cms () < AUTOROTATION_MIN_MOVING_SPEED;
99+ if (motors->get_below_land_min_coll () && AP::ins ().is_still () && speed_check) {
100+ phase_switch = Autorotation_Phase::LANDED;
101+ }
105102
103+ // Check if we are bailing out and need to re-set the spool state
104+ if (motors->autorotation_bailout ()) {
105+ motors->set_desired_spool_state (AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
106106 }
107107
108108
@@ -199,79 +199,22 @@ void ModeAutorotate::run()
199199 {
200200 break ;
201201 }
202-
203- case Autorotation_Phase::BAIL_OUT:
202+ case Autorotation_Phase::LANDED:
204203 {
205- if (_flags. bail_out_initial == true ) {
206- // Functions and settings to be done once are done here.
204+ // Entry phase functions to be run only once
205+ if (_flags. landed_initial == true ) {
207206
208207 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
209- gcs ().send_text (MAV_SEVERITY_INFO, " Bailing Out of Autorotation "
208+ gcs ().send_text (MAV_SEVERITY_INFO, " Landed "
210209 #endif
211-
212- // Set bail out timer remaining equal to the parameter value, bailout time
213- // cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME.
214- _bail_time = MAX (g2.arot .get_bail_time (),BAILOUT_MOTOR_RAMP_TIME+0 .1f );
215-
216- // Set bail out start time
217- _bail_time_start_ms = now;
218-
219- // Set initial target vertical speed
220- _desired_v_z = curr_vel_z;
221-
222- // Initialise position and desired velocity
223- if (!pos_control->is_active_z ()) {
224- pos_control->relax_z_controller (g2.arot .get_last_collective ());
225- }
226-
227- // Get pilot parameter limits
228- const float pilot_spd_dn = -get_pilot_speed_dn ();
229- const float pilot_spd_up = g.pilot_speed_up ;
230-
231- float pilot_des_v_z = get_pilot_desired_climb_rate (channel_throttle->get_control_in ());
232- pilot_des_v_z = constrain_float (pilot_des_v_z, pilot_spd_dn, pilot_spd_up);
233-
234- // Calculate target climb rate adjustment to transition from bail out descent speed to requested climb rate on stick.
235- _target_climb_rate_adjust = (curr_vel_z - pilot_des_v_z)/(_bail_time - BAILOUT_MOTOR_RAMP_TIME); // accounting for 0.5s motor spool time
236-
237- // Calculate pitch target adjustment rate to return to level
238- _target_pitch_adjust = _pitch_target/_bail_time;
239-
240- // set vertical speed and acceleration limits
241- pos_control->set_max_speed_accel_z (curr_vel_z, pilot_spd_up, fabsf (_target_climb_rate_adjust));
242- pos_control->set_correction_speed_accel_z (curr_vel_z, pilot_spd_up, fabsf (_target_climb_rate_adjust));
243-
244- motors->set_desired_spool_state (AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
245-
246- _flags.bail_out_initial = false ;
210+ _flags.landed_initial = false ;
247211 }
248-
249- if ((now - _bail_time_start_ms)/1000 .0f >= BAILOUT_MOTOR_RAMP_TIME) {
250- // Update desired vertical speed and pitch target after the bailout motor ramp timer has completed
251- _desired_v_z -= _target_climb_rate_adjust*G_Dt;
252- _pitch_target -= _target_pitch_adjust*G_Dt;
253- }
254- // Set position controller
255- pos_control->set_pos_target_z_from_climb_rate_cm (_desired_v_z);
256-
257- // Update controllers
258- pos_control->update_z_controller ();
259-
260- if ((now - _bail_time_start_ms)/1000 .0f >= _bail_time) {
261- // Bail out timer complete. Change flight mode. Do not revert back to auto. Prevent aircraft
262- // from continuing mission and potentially flying further away after a power failure.
263- if (copter.prev_control_mode == Mode::Number::AUTO) {
264- set_mode (Mode::Number::ALT_HOLD, ModeReason::AUTOROTATION_BAILOUT);
265- } else {
266- set_mode (copter.prev_control_mode , ModeReason::AUTOROTATION_BAILOUT);
267- }
268- }
269-
270- break ;
212+ // don't allow controller to continually ask for more pitch to build speed when we are on the ground, decay to zero smoothly
213+ _pitch_target *= 0.95 ;
214+ break ;
271215 }
272216 }
273217
274-
275218 switch (nav_pos_switch) {
276219
277220 case Navigation_Decision::USER_CONTROL_STABILISED:
0 commit comments