diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index c493eda291..420b35ab74 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -716,12 +716,6 @@ void Copter::twentyfive_hz_logging() AP::ins().Write_IMU(); } -#if MODE_AUTOROTATE_ENABLED - if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) { - //update autorotation log - g2.arot.Log_Write_Autorotation(); - } -#endif #if HAL_GYROFFT_ENABLED if (should_log(MASK_LOG_FTN_FAST)) { gyro_fft.write_log_messages(); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index aec9a92b6e..b4ec54db88 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -164,6 +164,10 @@ #error AP_OAPathPlanner relies on AP_FENCE_ENABLED which is disabled #endif +#if MODE_AUTOROTATE_ENABLED && !AP_RPM_ENABLED + #error AC_Autorotation relies on AP_RPM_ENABLED which is disabled +#endif + #if HAL_ADSB_ENABLED #include "avoidance_adsb.h" #endif diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index e8ec6e0620..4ce47bcf60 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1293,7 +1293,7 @@ ParametersG2::ParametersG2(void) : ,mode_systemid_ptr(&copter.mode_systemid) #endif #if MODE_AUTOROTATE_ENABLED - ,arot() + ,arot(copter.motors, copter.attitude_control) #endif #if MODE_ZIGZAG_ENABLED ,mode_zigzag_ptr(&copter.mode_zigzag) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 7ced9b0575..bc38167cbe 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -2033,46 +2033,21 @@ protected: private: - // --- Internal variables --- - float _initial_rpm; // Head speed recorded at initiation of flight mode (RPM) - float _target_head_speed; // The terget head main rotor head speed. Normalised by main rotor set point - float _desired_v_z; // Desired vertical - 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 + uint32_t _entry_time_start_ms; // time remaining until entry phase moves on to glide phase + uint32_t _last_logged_ms; // used for timing slow rate autorotation log - enum class Autorotation_Phase { + enum class Phase { + ENTRY_INIT, ENTRY, - SS_GLIDE, + GLIDE_INIT, + GLIDE, + FLARE_INIT, FLARE, + TOUCH_DOWN_INIT, TOUCH_DOWN, - LANDED } phase_switch; - - enum class Navigation_Decision { - USER_CONTROL_STABILISED, - STRAIGHT_AHEAD, - INTO_WIND, - NEAREST_RALLY} nav_pos_switch; - - // --- Internal flags --- - struct controller_flags { - bool entry_initial : 1; - 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 bad_rpm : 1; - } _flags; - - struct message_flags { - bool bad_rpm : 1; - } _msg_flags; - - //--- Internal functions --- - void warning_message(uint8_t message_n); //Handles output messages to the terminal + LANDED_INIT, + LANDED, + } current_phase; }; #endif diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 6bb7d42025..3f9e779dbf 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -9,14 +9,8 @@ #include #include "mode.h" -#include - #if MODE_AUTOROTATE_ENABLED -#define AUTOROTATE_ENTRY_TIME 2.0f // (s) number of seconds that the entry phase operates for -#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) { #if FRAME_CONFIG != HELI_FRAME @@ -27,77 +21,55 @@ bool ModeAutorotate::init(bool ignore_checks) // 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.enabled()) { - gcs().send_text(MAV_SEVERITY_WARNING, "Autorot Mode Not Enabled"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AROT: Mode not enabled"); return false; } // 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"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AROT: Must be armed and flying"); return false; } - // Initialise controllers - // This must be done before RPM value is fetched - g2.arot.init_hs_controller(); - g2.arot.init_fwd_spd_controller(); + // Initialise controller + g2.arot.init(); - // Retrieve rpm and start rpm sensor health checks - _initial_rpm = g2.arot.get_rpm(true); - - // Display message - gcs().send_text(MAV_SEVERITY_INFO, "Autorotation initiated"); - - // Set all intial flags to on - _flags.entry_initial = true; - _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; - _msg_flags.bad_rpm = true; - - // Setting default starting switches - phase_switch = Autorotation_Phase::ENTRY; + // Setting default starting state + current_phase = Phase::ENTRY_INIT; // Set entry timer _entry_time_start_ms = millis(); - // The decay rate to reduce the head speed from the current to the target - _hs_decay = ((_initial_rpm/g2.arot.get_hs_set_point()) - HEAD_SPEED_TARGET_RATIO) / AUTOROTATE_ENTRY_TIME; + // reset logging timer + _last_logged_ms = 0; return true; } - - void ModeAutorotate::run() { // Current time - uint32_t now = millis(); //milliseconds + const uint32_t now_ms = millis(); + + // Set dt in library + float const last_loop_time_s = AP::scheduler().get_last_loop_time_s(); + g2.arot.set_dt(last_loop_time_s); //---------------------------------------------------------------- // State machine logic //---------------------------------------------------------------- + // State machine progresses through the autorotation phases as you read down through the if statements. + // More urgent phases (the ones closer to the ground) take precedence later in the if statements. - // Setting default phase switch positions - nav_pos_switch = Navigation_Decision::USER_CONTROL_STABILISED; - - // 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; - } + if (current_phase < Phase::GLIDE_INIT && ((now_ms - _entry_time_start_ms) > g2.arot.entry_time_ms)) { + // Flight phase can be progressed to steady state glide + current_phase = Phase::GLIDE_INIT; } - // 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 believe we have landed. We need the landed state to zero all + // controls and make sure that the copter landing detector will trip + if (current_phase < Phase::LANDED && g2.arot.check_landed()) { + current_phase = Phase::LANDED_INIT; } // Check if we are bailing out and need to re-set the spool state @@ -105,162 +77,60 @@ void ModeAutorotate::run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } + // Get norm input from yaw channel + const float pilot_norm_input = channel_yaw->norm_input_dz(); //---------------------------------------------------------------- // State machine actions //---------------------------------------------------------------- - switch (phase_switch) { + switch (current_phase) { - case Autorotation_Phase::ENTRY: - { + case Phase::ENTRY_INIT: // Entry phase functions to be run only once - if (_flags.entry_initial == true) { - - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase"); - #endif - - // Set following trim low pass cut off frequency - g2.arot.set_col_cutoff_freq(g2.arot.get_col_entry_freq()); - - // Target head speed is set to rpm at initiation to prevent unwanted changes in attitude - _target_head_speed = _initial_rpm/g2.arot.get_hs_set_point(); - - // Set desired forward speed target - g2.arot.set_desired_fwd_speed(); - - // Prevent running the initial entry functions again - _flags.entry_initial = false; - - } - - // Slowly change the target head speed until the target head speed matches the parameter defined value - if (g2.arot.get_rpm() > HEAD_SPEED_TARGET_RATIO*1.005f || g2.arot.get_rpm() < HEAD_SPEED_TARGET_RATIO*0.995f) { - _target_head_speed -= _hs_decay*G_Dt; - } else { - _target_head_speed = HEAD_SPEED_TARGET_RATIO; - } - - // Set target head speed in head speed controller - g2.arot.set_target_head_speed(_target_head_speed); - - // Run airspeed/attitude controller - g2.arot.set_dt(G_Dt); - g2.arot.update_forward_speed_controller(); - - // Retrieve pitch target - _pitch_target = g2.arot.get_pitch(); - - // Update controllers - _flags.bad_rpm = g2.arot.update_hs_glide_controller(G_Dt); //run head speed/ collective controller + g2.arot.init_entry(); + current_phase = Phase::ENTRY; + FALLTHROUGH; + case Phase::ENTRY: + // Smoothly transition the collective to enter autorotation regime and begin forward speed control + g2.arot.run_entry(pilot_norm_input); break; - } - case Autorotation_Phase::SS_GLIDE: - { - // Steady state glide functions to be run only once - if (_flags.ss_glide_initial == true) { - - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase"); - #endif - - // Set following trim low pass cut off frequency - g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq()); - - // Set desired forward speed target - g2.arot.set_desired_fwd_speed(); - - // Set target head speed in head speed controller - _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide in case hs has not reached target for glide - g2.arot.set_target_head_speed(_target_head_speed); - - // Prevent running the initial glide functions again - _flags.ss_glide_initial = false; - } - - // Run airspeed/attitude controller - g2.arot.set_dt(G_Dt); - g2.arot.update_forward_speed_controller(); - - // Retrieve pitch target - _pitch_target = g2.arot.get_pitch(); - - // Update head speed/ collective controller - _flags.bad_rpm = g2.arot.update_hs_glide_controller(G_Dt); - // Attitude controller is updated in navigation switch-case statements + case Phase::GLIDE_INIT: + // Glide phase functions to be run only once + g2.arot.init_glide(); + current_phase = Phase::GLIDE; + FALLTHROUGH; + case Phase::GLIDE: + // Maintain head speed and forward speed as we glide to the ground + g2.arot.run_glide(pilot_norm_input); break; - } - case Autorotation_Phase::FLARE: - case Autorotation_Phase::TOUCH_DOWN: - { + case Phase::FLARE_INIT: + case Phase::FLARE: + case Phase::TOUCH_DOWN_INIT: + case Phase::TOUCH_DOWN: break; - } - case Autorotation_Phase::LANDED: - { - // 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, "Landed"); - #endif - _flags.landed_initial = false; - } - // 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; + case Phase::LANDED_INIT: + // Landed phase functions to be run only once + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AROT: Landed"); + current_phase = Phase::LANDED; + FALLTHROUGH; + + case Phase::LANDED: + // Don't allow controller to continually ask for more pitch to build speed when we are on the ground, decay to zero smoothly + g2.arot.run_landed(); break; - } } - switch (nav_pos_switch) { - - case Navigation_Decision::USER_CONTROL_STABILISED: - { - // Operator is in control of roll and yaw. Controls act as if in stabilise flight mode. Pitch - // is controlled by speed-height controller. - float pilot_roll, pilot_pitch; - get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max); - - // Get pilot's desired yaw rate - float pilot_yaw_rate = get_pilot_desired_yaw_rate(); - - // Pitch target is calculated in autorotation phase switch above - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pilot_roll, _pitch_target, pilot_yaw_rate); - break; - } - - case Navigation_Decision::STRAIGHT_AHEAD: - case Navigation_Decision::INTO_WIND: - case Navigation_Decision::NEAREST_RALLY: - { - break; - } - } - - // Output warning messaged if rpm signal is bad - if (_flags.bad_rpm) { - warning_message(1); + // Slow rate (25 Hz) logging for the mode + if (now_ms - _last_logged_ms > 40U) { + g2.arot.log_write_autorotation(); + _last_logged_ms = now_ms; } } // End function run() -void ModeAutorotate::warning_message(uint8_t message_n) -{ - switch (message_n) { - case 1: - { - if (_msg_flags.bad_rpm) { - // Bad rpm sensor health. - gcs().send_text(MAV_SEVERITY_INFO, "Warning: Poor RPM Sensor Health"); - gcs().send_text(MAV_SEVERITY_INFO, "Action: Minimum Collective Applied"); - _msg_flags.bad_rpm = false; - } - break; - } - } -} - #endif