diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index d1de6fe45d78fc..901060e9fee015 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1283,7 +1283,7 @@ ParametersG2::ParametersG2(void) ,mode_systemid_ptr(&copter.mode_systemid) #endif #if MODE_AUTOROTATE_ENABLED == ENABLED - ,arot() + ,arot(copter.inertial_nav, copter.ahrs) #endif #if HAL_BUTTON_ENABLED ,button_ptr(&copter.button) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 97793741c9895f..d695767a8234c5 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -279,17 +279,13 @@ ////////////////////////////////////////////////////////////////////////////// // Autorotate - autonomous auto-rotation - helicopters only #ifndef MODE_AUTOROTATE_ENABLED -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - #if FRAME_CONFIG == HELI_FRAME - #ifndef MODE_AUTOROTATE_ENABLED - # define MODE_AUTOROTATE_ENABLED ENABLED - #endif - #else - # define MODE_AUTOROTATE_ENABLED DISABLED - #endif -#else - # define MODE_AUTOROTATE_ENABLED DISABLED -#endif + #if FRAME_CONFIG == HELI_FRAME + #ifndef MODE_AUTOROTATE_ENABLED + # define MODE_AUTOROTATE_ENABLED ENABLED + #endif + #else + # define MODE_AUTOROTATE_ENABLED DISABLED + #endif #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index a9c9fde1ac7cfa..d8cdc4402c9c94 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -196,6 +196,19 @@ void Copter::heli_update_rotor_speed_targets() // to autorotation flight mode if manual collective is not being used. void Copter::heli_update_autorotation() { +#if MODE_AUTOROTATE_ENABLED == ENABLED + // Always update the ground distance to prevent a race on init + if (motors->armed() && g2.arot.is_enable()) { + // Get height above ground. If using a healthy LiDaR below func will return an interpolated + // distance based on inertial measurement. If LiDaR is unhealthy and terrain is available + // we will get a terrain database estimate. Otherwise we will get height above home. + int32_t gnd_dist = flightmode->get_alt_above_ground_cm(false); + + // set the height in the autorotation controller + g2.arot.set_ground_distance(gnd_dist); + } +#endif + // check if flying and interlock disengaged if (!ap.land_complete && !motors->get_interlock()) { #if MODE_AUTOROTATE_ENABLED == ENABLED @@ -204,7 +217,7 @@ void Copter::heli_update_autorotation() // set autonomous autorotation flight mode set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START); } - // set flag to facilitate both auto and manual autorotations + // set flag to facilitate auto autorotation heli_flags.in_autorotation = true; motors->set_in_autorotation(heli_flags.in_autorotation); motors->set_enable_bailout(true); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index f235cc22ef3906..9fcdfe6c2988a1 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -609,13 +609,13 @@ void Mode::make_safe_ground_handling(bool force_throttle_unlimited) /* get a height above ground estimate for landing */ -int32_t Mode::get_alt_above_ground_cm(void) +int32_t Mode::get_alt_above_ground_cm(bool use_psc_case) { int32_t alt_above_ground_cm; if (copter.get_rangefinder_height_interpolated_cm(alt_above_ground_cm)) { return alt_above_ground_cm; } - if (!pos_control->is_active_xy()) { + if (!pos_control->is_active_xy() && use_psc_case) { return copter.current_loc.alt; } if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_cm)) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index dd06ee0149c296..97a54aae5ef9a8 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -152,7 +152,7 @@ class Mode { virtual bool set_speed_up(float speed_xy_cms) {return false;} virtual bool set_speed_down(float speed_xy_cms) {return false;} - int32_t get_alt_above_ground_cm(void); + int32_t get_alt_above_ground_cm(bool use_psc_case=true); // pilot input processing void get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const; @@ -1934,39 +1934,29 @@ class ModeAutorotate : public Mode { 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 + uint32_t _touchdown_time_ms; + bool _hover_autorotation; // bool to determine if we should enter the hover autorotation or not enum class Autorotation_Phase { ENTRY, + HOVER_ENTRY, SS_GLIDE, FLARE, TOUCH_DOWN, BAIL_OUT } 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 straight_ahead_initial : 1; - bool level_initial : 1; - bool break_initial : 1; - bool bail_out_initial : 1; - bool bad_rpm : 1; + bool entry_init : 1; + bool hover_entry_init : 1; + bool ss_glide_init : 1; + bool flare_init : 1; + bool touch_down_init : 1; + bool bail_out_init : 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 + uint32_t _last_bad_rpm_ms; }; #endif diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 85f278ac0d801d..34979b28c9f6bf 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -13,9 +13,10 @@ #if MODE_AUTOROTATE_ENABLED == 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 AUTOROTATE_ENTRY_TIME 2000 // (ms) Number of milliseconds 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 +#define MSG_TIMER 7000 // (ms) time interval between sending repeat warning messages bool ModeAutorotate::init(bool ignore_checks) { @@ -36,27 +37,26 @@ bool ModeAutorotate::init(bool ignore_checks) return false; } - // Initialise controllers - // This must be done before RPM value is fetched - g2.arot.init_hs_controller(); - g2.arot.init_fwd_spd_controller(); + // Init autorotation controllers object + g2.arot.init(motors, copter.rangefinder.ground_clearance_cm_orient(ROTATION_PITCH_270)); // Retrieve rpm and start rpm sensor health checks - _initial_rpm = g2.arot.get_rpm(true); + _initial_rpm = g2.arot.get_rpm(); - // Display message + // 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.level_initial = true; - _flags.break_initial = true; - _flags.straight_ahead_initial = true; - _flags.bail_out_initial = true; - _msg_flags.bad_rpm = true; + // Set all intial flags to on + _flags.entry_init = false; + _flags.hover_entry_init = false; + _flags.ss_glide_init = false; + _flags.flare_init = false; + _flags.touch_down_init = false; + _flags.bail_out_init = false; + + // Check if we have sufficient speed or height to do a full autorotation otherwise we have to do one from the hover + // Note: This must be called after arot.init() + _hover_autorotation = (inertial_nav.get_speed_xy_cms() < 250.0f) && !g2.arot.above_flare_height(); // Setting default starting switches phase_switch = Autorotation_Phase::ENTRY; @@ -65,44 +65,56 @@ bool ModeAutorotate::init(bool ignore_checks) _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; + _hs_decay = ((_initial_rpm/g2.arot.get_hs_set_point()) - HEAD_SPEED_TARGET_RATIO) / (AUTOROTATE_ENTRY_TIME*1e-3); return true; } - 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 + // 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); + + float curr_vel_z = inertial_nav.get_velocity_z_up_cms(); //---------------------------------------------------------------- // 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. Init + // flags are used to prevent flight phase regression - // Setting default phase switch positions - nav_pos_switch = Navigation_Decision::USER_CONTROL_STABILISED; + if (!_hover_autorotation && !_flags.ss_glide_init && g2.arot.above_flare_height() && ((now - _entry_time_start_ms) > AUTOROTATE_ENTRY_TIME)) { + // Flight phase can be progressed to steady state glide + phase_switch = Autorotation_Phase::SS_GLIDE; + } - // Timer from entry phase to progress to glide phase - if (phase_switch == Autorotation_Phase::ENTRY){ + // Check if we are between the flare start height and the touchdown height + if (!_hover_autorotation && !_flags.flare_init && !g2.arot.above_flare_height() && !g2.arot.should_begin_touchdown()) { + phase_switch = Autorotation_Phase::FLARE; + } - 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; - } + // Initial check to see if we need to perform a hover autorotation + if (_hover_autorotation && !_flags.hover_entry_init) { + phase_switch = Autorotation_Phase::HOVER_ENTRY; + } + // Begin touch down if within touch down time + if (!_flags.touch_down_init && g2.arot.should_begin_touchdown()) { + phase_switch = Autorotation_Phase::TOUCH_DOWN; + } + + // 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); } @@ -111,146 +123,189 @@ void ModeAutorotate::run() //---------------------------------------------------------------- switch (phase_switch) { - case Autorotation_Phase::ENTRY: - { - // Entry phase functions to be run only once - if (_flags.entry_initial == true) { + case Autorotation_Phase::ENTRY: { + // Entry phase functions to be run only once + if (!_flags.entry_init) { + gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase"); - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase"); - #endif + // Set necessary variables for the entry phase in the autorotation obj + g2.arot.init_entry(); - // 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(); - // 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(); + // Prevent running the initial entry functions again + _flags.entry_init = true; + } - // Set desired forward speed target - g2.arot.set_desired_fwd_speed(); + // Slowly change the target head speed until the target head speed matches the parameter defined value + float norm_rpm = g2.arot.get_rpm()/g2.arot.get_hs_set_point(); - // Prevent running the initial entry functions again - _flags.entry_initial = false; + if (norm_rpm > HEAD_SPEED_TARGET_RATIO*1.005f || norm_rpm < HEAD_SPEED_TARGET_RATIO*0.995f) { + _target_head_speed -= _hs_decay * last_loop_time_s; + } 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); - // 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; - } + // Run airspeed/attitude controller + g2.arot.update_forward_speed_controller(); + + // Retrieve pitch target + _pitch_target = g2.arot.get_pitch(); + + // Update head speed controllers + g2.arot.update_hs_glide_controller(); + + break; + } + + case Autorotation_Phase::HOVER_ENTRY: { + // Controller phase where the aircraft is too low and too slow to perform a full autorotation + // instead, we will try to minimize rotor drag until we can jump to the touch down phase + if (!_flags.hover_entry_init) { + gcs().send_text(MAV_SEVERITY_INFO, "Hover Entry Phase"); + _flags.hover_entry_init = true; + } + _pitch_target = 0.0f; + g2.arot.update_hover_autorotation_controller(); + break; + } - // Set target head speed in head speed controller - g2.arot.set_target_head_speed(_target_head_speed); + case Autorotation_Phase::SS_GLIDE: { + // Steady state glide functions to be run only once + if (!_flags.ss_glide_init) { - // Run airspeed/attitude controller - g2.arot.set_dt(G_Dt); - g2.arot.update_forward_speed_controller(); + gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase"); - // Retrieve pitch target - _pitch_target = g2.arot.get_pitch(); + // Ensure target hs is set to glide in case hs hasn't reached target for glide + _target_head_speed = HEAD_SPEED_TARGET_RATIO; - // Update controllers - _flags.bad_rpm = g2.arot.update_hs_glide_controller(G_Dt); //run head speed/ collective controller + // Set necessary variables for the glide phase in the autorotation obj + g2.arot.init_glide(_target_head_speed); - break; + // Prevent running the initial glide functions again + _flags.ss_glide_init = true; } - case Autorotation_Phase::SS_GLIDE: - { - // Steady state glide functions to be run only once - if (_flags.ss_glide_initial == true) { + // Run airspeed/attitude controller + g2.arot.update_forward_speed_controller(); - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase"); - #endif + //update flare altitude estimate + g2.arot.update_flare_alt(); - // Set following trim low pass cut off frequency - g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq()); + // Retrieve pitch target + _pitch_target = g2.arot.get_pitch(); - // Set desired forward speed target - g2.arot.set_desired_fwd_speed(); + // Update head speed/ collective controller + g2.arot.update_hs_glide_controller(); - // 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); + // Attitude controller is updated in navigation switch-case statements + g2.arot.update_avg_acc_z(); - // Prevent running the initial glide functions again - _flags.ss_glide_initial = false; - } + break; + } - // Run airspeed/attitude controller - g2.arot.set_dt(G_Dt); - g2.arot.update_forward_speed_controller(); + case Autorotation_Phase::FLARE: { + // Steady state glide functions to be run only once + if (!_flags.flare_init) { + gcs().send_text(MAV_SEVERITY_INFO, "Flare_Phase"); - // Retrieve pitch target - _pitch_target = g2.arot.get_pitch(); + // Ensure target head speed is set in head speed controller + _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide incase hs has not reached target for glide - // 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 + g2.arot.init_flare(_target_head_speed); - break; + // Prevent running the initial flare functions again + _flags.flare_init = true; } + // Run flare controller + g2.arot.flare_controller(); - case Autorotation_Phase::FLARE: - case Autorotation_Phase::TOUCH_DOWN: - { - break; - } + // Update head speed/ collective controller + g2.arot.update_hs_glide_controller(); - case Autorotation_Phase::BAIL_OUT: - { - if (_flags.bail_out_initial == true) { - // Functions and settings to be done once are done here. + // Retrieve pitch target + _pitch_target = g2.arot.get_pitch(); - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation"); - #endif + //calc average acceleration on z axis for estimating flare effectiveness + g2.arot.update_avg_acc_z(); - // 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); + break; + } + + case Autorotation_Phase::TOUCH_DOWN: { + if (!_flags.touch_down_init) { + gcs().send_text(MAV_SEVERITY_INFO, "Touchdown_Phase"); - // Set bail out start time - _bail_time_start_ms = now; + // Save the time on init of touchdown + _touchdown_time_ms = millis(); - // Set initial target vertical speed - _desired_v_z = curr_vel_z; + g2.arot.init_touchdown(); + + // Prevent running the initial touchdown functions again + _flags.touch_down_init = true; + } - // Initialise position and desired velocity - if (!pos_control->is_active_z()) { - pos_control->relax_z_controller(g2.arot.get_last_collective()); - } + // Run touchdown controller + g2.arot.touchdown_controller(); - // Get pilot parameter limits - const float pilot_spd_dn = -get_pilot_speed_dn(); - const float pilot_spd_up = g.pilot_speed_up; + // Retrieve pitch target + _pitch_target = g2.arot.get_pitch(); - 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); + break; + } - // 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 + case Autorotation_Phase::BAIL_OUT: { + if (!_flags.bail_out_init) { + // Functions and settings to be done once are done here. + gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation"); - // Calculate pitch target adjustment rate to return to level - _target_pitch_adjust = _pitch_target/_bail_time; + // 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 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)); + // Set bail out start time + _bail_time_start_ms = now; - motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + // Set initial target vertical speed + _desired_v_z = curr_vel_z; - _flags.bail_out_initial = false; + // 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_init = true; + } + 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; + _desired_v_z -= _target_climb_rate_adjust * last_loop_time_s; + _pitch_target -= _target_pitch_adjust * last_loop_time_s; } + // Set position controller pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z); @@ -268,56 +323,46 @@ void ModeAutorotate::run() } 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(channel_yaw->norm_input_dz()); - - // 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); + float pilot_roll, pilot_pitch, pilot_yaw_rate, target_roll; + // Operator is in control of roll and yaw. Pitch is controlled by speed-height controller. + get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max); + + // Allow pilot control of the roll and yaw. Two control schemes available, a stabilise-like control scheme + // and a loiter like control scheme. + if (g2.arot.use_stabilise_controls()) { + // Get pilot's desired yaw rate + pilot_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + target_roll = pilot_roll; + + } else { + // loiter-like control scheme + // rotate roll, pitch input from north facing to vehicle's perspective + Vector2f vel = ahrs.earth_to_body2D(inertial_nav.get_velocity_neu_cms().xy()); + float roll_vel = vel.y; + float pitch_vel = vel.x; + + // gain scheduling for yaw + // TODO: Need to change consts to sensibly named defines + float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); + pilot_yaw_rate = ((float)pilot_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g2.command_model_pilot.get_rate() / 45.0; + + roll_vel = constrain_float(roll_vel, -560.0f, 560.0f); + pitch_vel = constrain_float(pitch_vel, -560.0f, 560.0f); + + // convert user input into desired roll velocity + float roll_vel_error = roll_vel - (pilot_roll / 0.8f); + + // roll velocity is feed into roll acceleration to minimize slip + target_roll = roll_vel_error * -0.8f; + target_roll = constrain_float(target_roll, -4500.0f, 4500.0f); } -} // End function run() + // Pitch target is calculated in autorotation phase switch above + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, _pitch_target, pilot_yaw_rate); -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; - } - } -} +} // End function run() #endif diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 627dd7894ec317..00b7b44708d7a2 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -2,22 +2,19 @@ #include #include #include - -//Autorotation controller defaults -#define AROT_BAIL_OUT_TIME 2.0f // Default time for bail out controller to run (unit: s) +#include // Head Speed (HS) controller specific default definitions #define HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ 2.0f // low-pass filter on accel error (unit: hz) #define HS_CONTROLLER_HEADSPEED_P 0.7f // Default P gain for head speed controller (unit: -) -#define HS_CONTROLLER_ENTRY_COL_FILTER 0.7f // Default low pass filter frequency during the entry phase (unit: Hz) -#define HS_CONTROLLER_GLIDE_COL_FILTER 0.1f // Default low pass filter frequency during the glide phase (unit: Hz) // Speed Height controller specific default definitions for autorotation use -#define FWD_SPD_CONTROLLER_GND_SPEED_TARGET 1100 // Default target ground speed for speed height controller (unit: cm/s) -#define FWD_SPD_CONTROLLER_MAX_ACCEL 60 // Default acceleration limit for speed height controller (unit: cm/s/s) -#define AP_FW_VEL_P 0.9f -#define AP_FW_VEL_FF 0.15f +#define AP_FW_VEL_P 0.9f // Default forward speed controller P gain +#define TCH_P 0.1f // Default touchdown phase collective controller P gain +// flare controller default definitions +#define AP_ALPHA_TPP 20.0f // (deg) Maximum angle of the Tip Path Plane +#define MIN_TIME_ON_GROUND 3000 // (ms) Time on ground required before collective is slewed to zero thrust const AP_Param::GroupInfo AC_Autorotation::var_info[] = { @@ -52,7 +49,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 800 2000 // @Increment: 50 // @User: Advanced - AP_GROUPINFO("TARG_SP", 4, AC_Autorotation, _param_target_speed, FWD_SPD_CONTROLLER_GND_SPEED_TARGET), + AP_GROUPINFO("TARG_SP", 4, AC_Autorotation, _param_target_speed, 1100), // @Param: COL_FILT_E // @DisplayName: Entry Phase Collective Filter @@ -61,7 +58,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.2 0.5 // @Increment: 0.01 // @User: Advanced - AP_GROUPINFO("COL_FILT_E", 5, AC_Autorotation, _param_col_entry_cutoff_freq, HS_CONTROLLER_ENTRY_COL_FILTER), + AP_GROUPINFO("COL_FILT_E", 5, AC_Autorotation, _param_col_entry_cutoff_freq, 0.7), // @Param: COL_FILT_G // @DisplayName: Glide Phase Collective Filter @@ -70,7 +67,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.03 0.15 // @Increment: 0.01 // @User: Advanced - AP_GROUPINFO("COL_FILT_G", 6, AC_Autorotation, _param_col_glide_cutoff_freq, HS_CONTROLLER_GLIDE_COL_FILTER), + AP_GROUPINFO("COL_FILT_G", 6, AC_Autorotation, _param_col_glide_cutoff_freq, 0.1), // @Param: AS_ACC_MAX // @DisplayName: Forward Acceleration Limit @@ -79,7 +76,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 30 60 // @Increment: 10 // @User: Advanced - AP_GROUPINFO("AS_ACC_MAX", 7, AC_Autorotation, _param_accel_max, FWD_SPD_CONTROLLER_MAX_ACCEL), + AP_GROUPINFO("AS_ACC_MAX", 7, AC_Autorotation, _param_accel_max, 60), // @Param: BAIL_TIME // @DisplayName: Bail Out Timer @@ -88,10 +85,10 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.5 4 // @Increment: 0.1 // @User: Advanced - AP_GROUPINFO("BAIL_TIME", 8, AC_Autorotation, _param_bail_time, AROT_BAIL_OUT_TIME), + AP_GROUPINFO("BAIL_TIME", 8, AC_Autorotation, _param_bail_time, 2.0), // @Param: HS_SENSOR - // @DisplayName: Main Rotor RPM Sensor + // @DisplayName: Main Rotor RPM Sensor // @Description: Allocate the RPM sensor instance to use for measuring head speed. RPM1 = 0. RPM2 = 1. // @Units: s // @Range: 0.5 3 @@ -113,208 +110,361 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0 1 // @Increment: 0.01 // @User: Advanced - AP_GROUPINFO("FW_V_FF", 11, AC_Autorotation, _param_fwd_k_ff, AP_FW_VEL_FF), + AP_GROUPINFO("FW_V_FF", 11, AC_Autorotation, _param_fwd_k_ff, 0.15), + + // @Param: TCH_P + // @DisplayName: P gain for vertical touchdown controller + // @Description: proportional term based on sink rate error + // @Range: 0.3 1 + // @Increment: 0.01 + // @User: Advanced + AP_SUBGROUPINFO(_p_coll_tch, "TCH_", 12, AC_Autorotation, AC_P), + + // @Param: COL_FILT_C + // @DisplayName: Touchdown Phase Collective Filter + // @Description: Cut-off frequency for collective low pass filter. For the touchdown phase. Acts as a following trim. + // @Units: Hz + // @Range: 0.2 0.8 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO("COL_FILT_C", 13, AC_Autorotation, _param_col_touchdown_cutoff_freq, 0.5), + + // @Param: ROT_SOL + // @DisplayName: rotor solidity + // @Description: helicopter specific main rotor solidity + // @Range: 0.001 0.01 + // @Increment: 0.001 + // @User: Advanced + AP_GROUPINFO("ROT_SOL", 14, AC_Autorotation, _param_solidity, 0.05), + + // @Param: ROT_DIAM + // @DisplayName: rotor diameter + // @Description: helicopter specific main rotor diameter + // @Units: m + // @Range: 0.001 0.01 + // @Increment: 0.001 + // @User: Advanced + AP_GROUPINFO("ROT_DIAM", 15, AC_Autorotation, _param_diameter, 1.25), + + // @Param: T_TCH + // @DisplayName: time touchdown + // @Description: time touchdown + // @Units: s + // @Range: 0.001 0.01 + // @Increment: 0.001 + // @User: Advanced + AP_GROUPINFO("T_TCH", 16, AC_Autorotation, _t_tch, 0.55), + + // @Param: OPTIONS + // @DisplayName: Autorotation options + // @Description: Bitmask for autorotation options. + // @Bitmask: 0: Use stabilize-like controls (roll angle, yaw rate) + AP_GROUPINFO("OPTIONS", 17, AC_Autorotation, _options, 0), AP_GROUPEND }; + // Constructor -AC_Autorotation::AC_Autorotation() : +AC_Autorotation::AC_Autorotation(AP_InertialNav& inav, AP_AHRS& ahrs) : + _inav(inav), + _ahrs(ahrs), _p_hs(HS_CONTROLLER_HEADSPEED_P), - _p_fw_vel(AP_FW_VEL_P) - { - AP_Param::setup_object_defaults(this, var_info); + _p_fw_vel(AP_FW_VEL_P), + _p_coll_tch(TCH_P) +{ + AP_Param::setup_object_defaults(this, var_info); +} + + +void AC_Autorotation::init(AP_MotorsHeli* motors, float gnd_clear) { + + _motors_heli = motors; + if (_motors_heli == nullptr) { + AP_HAL::panic("AROT: _motors_heli is nullptr"); } -// Initialisation of head speed controller -void AC_Autorotation::init_hs_controller() -{ - // Set initial collective position to be the collective position on initialisation - _collective_out = 0.4f; + // Reset z acceleration average variables + _avg_acc_z = 0.0f; + _acc_z_sum = 0.0f; + _index = 0; + memset(_curr_acc_z, 0, sizeof(_curr_acc_z)); + + initial_flare_estimate(); + + // Initialisation of head speed controller + // Set initial collective position to be the zero thrust collective and minimize loss in head speed + _collective_out = _motors_heli->get_coll_mid(); // Reset feed forward filter col_trim_lpf.reset(_collective_out); - // Reset flags - _flags.bad_rpm = false; + // Protect against divide by zero + _param_head_speed_set_point.set(MAX(_param_head_speed_set_point, 500.0)); - // Reset RPM health monitoring - _unhealthy_rpm_counter = 0; - _healthy_rpm_counter = 0; + // Initialise forward speed controller + _accel_target = 0.0; - // Protect against divide by zero - _param_head_speed_set_point.set(MAX(_param_head_speed_set_point,500)); + // Ensure parameter acceleration doesn't exceed hard-coded limit + _accel_max = MIN(_param_accel_max, 500.0); + + // Reset cmd vel and last accel to sensible values + _cmd_vel = calc_speed_forward(); //(cm/s) + _accel_out_last = _cmd_vel * _param_fwd_k_ff; + + // Store the ground clearance on the lidar sensor for use in touch down calculations + _ground_clearance = gnd_clear; + + // reset on ground timer + _time_on_ground = 0; +} + + +void AC_Autorotation::init_entry(void) +{ + // Set following trim low pass cut off frequency + _col_cutoff_freq = _param_col_entry_cutoff_freq.get(); + + // Set desired forward speed target + _vel_target = _param_target_speed.get(); } +void AC_Autorotation::init_glide(float hs_targ) +{ + // Set following trim low pass cut off frequency + _col_cutoff_freq = _param_col_glide_cutoff_freq.get(); + + // Ensure desired forward speed target is set to param value + _vel_target = _param_target_speed.get(); + + // Update head speed target + _target_head_speed = hs_targ; +} + +void AC_Autorotation::init_flare(float hs_targ) +{ + // Ensure following trim low pass cut off frequency + _col_cutoff_freq = _param_col_glide_cutoff_freq.get(); + + _flare_entry_speed = calc_speed_forward(); -bool AC_Autorotation::update_hs_glide_controller(float dt) + // Update head speed target + _target_head_speed = hs_targ; +} + +void AC_Autorotation::init_touchdown(void) { - // Reset rpm health flag - _flags.bad_rpm = false; - _flags.bad_rpm_warning = false; + // Set following trim low pass cut off frequency + _col_cutoff_freq = _param_col_touchdown_cutoff_freq.get(); + // store the descent speed and height at the start of the touch down + _entry_sink_rate = _inav.get_velocity_z_up_cms(); + _entry_alt = _gnd_hgt; +} + + +// Rotor Speed controller for entry, glide and flare phases of autorotation +void AC_Autorotation::update_hs_glide_controller(void) +{ // Get current rpm and update healthy signal counters - _current_rpm = get_rpm(true); + _current_rpm = get_rpm(); - if (_unhealthy_rpm_counter <=30) { + if (_current_rpm > 0.0) { // Normalised head speed float head_speed_norm = _current_rpm / _param_head_speed_set_point; // Set collective trim low pass filter cut off frequency col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); - // Calculate the head speed error. Current rpm is normalised by the set point head speed. + // Calculate the head speed error. Current rpm is normalised by the set point head speed. // Target head speed is defined as a percentage of the set point. _head_speed_error = head_speed_norm - _target_head_speed; _p_term_hs = _p_hs.get_p(_head_speed_error); // Adjusting collective trim using feed forward (not yet been updated, so this value is the previous time steps collective position) - _ff_term_hs = col_trim_lpf.apply(_collective_out, dt); + _ff_term_hs = col_trim_lpf.apply(_collective_out, _dt); // Calculate collective position to be set - _collective_out = _p_term_hs + _ff_term_hs; + _collective_out = constrain_value((_p_term_hs + _ff_term_hs), 0.0f, 1.0f) ; } else { - // RPM sensor is bad set collective to minimum - _collective_out = -1.0f; - - _flags.bad_rpm_warning = true; + // RPM sensor is bad, set collective to angle of -2 deg and hope for the best + _collective_out = _motors_heli->calc_coll_from_ang(-2.0); } // Send collective to setting to motors output library set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); +} + + +void AC_Autorotation::update_hover_autorotation_controller() +{ + // Set collective trim low pass filter cut off frequency + col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); + + // Use zero thrust collective to minimize rotor speed loss + _ff_term_hs = col_trim_lpf.apply(_motors_heli->get_coll_mid(), _dt); - return _flags.bad_rpm_warning; + // Calculate collective position to be set + _collective_out = constrain_value(_ff_term_hs, 0.0f, 1.0f); + + // Send collective to setting to motors output library + set_collective(_col_cutoff_freq); } // Function to set collective and collective filter in motor library void AC_Autorotation::set_collective(float collective_filter_cutoff) const { - AP_Motors *motors = AP::motors(); - if (motors) { - motors->set_throttle_filter_cutoff(collective_filter_cutoff); - motors->set_throttle(_collective_out); - } + _motors_heli->set_throttle_filter_cutoff(collective_filter_cutoff); + _motors_heli->set_throttle(_collective_out); } -//function that gets rpm and does rpm signal checking to ensure signal is reliable -//before using it in the controller -float AC_Autorotation::get_rpm(bool update_counter) +// Function that gets rpm and does rpm signal checking to ensure signal is reliable +// before using it in the controller +float AC_Autorotation::get_rpm(void) { - float current_rpm = 0.0f; + float current_rpm = 0.0; #if AP_RPM_ENABLED // Get singleton for RPM library const AP_RPM *rpm = AP_RPM::get_singleton(); - //Get current rpm, checking to ensure no nullptr + // Get current rpm, checking to ensure no nullptr if (rpm != nullptr) { - //Check requested rpm instance to ensure either 0 or 1. Always defaults to 0. + // Check requested rpm instance to ensure either 0 or 1. Always defaults to 0. if ((_param_rpm_instance > 1) || (_param_rpm_instance < 0)) { _param_rpm_instance.set(0); } - //Get RPM value + // Get RPM value uint8_t instance = _param_rpm_instance; - //Check RPM sensor is returning a healthy status - if (!rpm->get_rpm(instance, current_rpm) || current_rpm <= -1) { - //unhealthy, rpm unreliable - _flags.bad_rpm = true; + // Check RPM sensor is returning a healthy status + if (!rpm->get_rpm(instance, current_rpm)) { + current_rpm = 0.0; } - - } else { - _flags.bad_rpm = true; } -#else - _flags.bad_rpm = true; #endif + return current_rpm; +} - if (_flags.bad_rpm) { - //count unhealthy rpm updates and reset healthy rpm counter - _unhealthy_rpm_counter++; - _healthy_rpm_counter = 0; - } else if (!_flags.bad_rpm && _unhealthy_rpm_counter > 0) { - //poor rpm reading may have cleared. Wait 10 update cycles to clear. - _healthy_rpm_counter++; +void AC_Autorotation::initial_flare_estimate(void) +{ + // Estimate hover thrust + float _col_hover_rad = radians(_motors_heli->get_hover_coll_ang()); + float b = _param_solidity * M_2PI; + _disc_area = M_PI * sq(_param_diameter * 0.5f); + float lambda = (-(b / 8.0f) + safe_sqrt((sq(b)) / 64.0f +((b / 3.0f) * _col_hover_rad))) * 0.5f; + float freq = _motors_heli->get_rpm_setpoint() / 60.0f; + float tip_speed= freq * M_2PI * _param_diameter * 0.5f; + _lift_hover = ((SSL_AIR_DENSITY * sq(tip_speed) * (_param_solidity * _disc_area)) * ((_col_hover_rad / 3.0f) - (lambda / 2.0f)) * 5.8f) * 0.5f; + + // Estimate rate of descent + float omega_auto = (_param_head_speed_set_point / 60.0f) * M_2PI; + float tip_speed_auto = omega_auto * _param_diameter * 0.5f; + float c_t = _lift_hover / (0.6125f * _disc_area * sq(tip_speed)); + _est_rod = ((0.25f * (_param_solidity * 0.011f / c_t) * tip_speed_auto) + ((sq(c_t) / (_param_solidity * 0.011f)) * tip_speed_auto)); + + // Estimate rotor C_d + _c = (_lift_hover / (sq(_est_rod) * 0.5f * SSL_AIR_DENSITY * _disc_area)) * 1.15f; + _c = constrain_float(_c, 0.7f, 1.4f); + + // Calc flare altitude + float des_spd_fwd = _param_target_speed * 0.01f; + calc_flare_alt(-_est_rod, des_spd_fwd); + + // Initialize flare bools + _flare_complete = false; + _flare_update_check = false; + + + gcs().send_text(MAV_SEVERITY_INFO, "Ct/sigma=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/GRAVITY_MSS, _flare_alt_calc*0.01f, _c); + gcs().send_text(MAV_SEVERITY_INFO, "est_rod=%f", _est_rod); +} - if (_healthy_rpm_counter >= 10) { - //poor rpm health has cleared, reset counters - _unhealthy_rpm_counter = 0; - _healthy_rpm_counter = 0; - } - } - return current_rpm; + +void AC_Autorotation::calc_flare_alt(float sink_rate, float fwd_speed) +{ + // Compute speed module and glide path angle during descent + float speed_module = norm(sink_rate, fwd_speed); + float glide_angle = safe_asin(M_PI / 2 - (fwd_speed / speed_module)); + + // Estimate inflow velocity at beginning of flare + float inflow = - speed_module * sinf(glide_angle + radians(AP_ALPHA_TPP)); + + // Estimate flare duration + float k_1 = fabsf((-sink_rate + 0.001f - safe_sqrt(_lift_hover / _c))/(-sink_rate + 0.001f + safe_sqrt(_lift_hover / _c))); + float k_2 = fabsf((inflow - safe_sqrt(_lift_hover / _c)) / (inflow + safe_sqrt(_lift_hover / _c))); + float delta_t_flare = (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) - (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_2)); + + // Estimate flare delta altitude + float sq_gravity = sq(GRAVITY_MSS); + float a = (2 * safe_sqrt(_c * sq_gravity / _lift_hover )) * delta_t_flare + (2 * safe_sqrt(_c * sq_gravity / _lift_hover )) * (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)); + float x = 1 - expf(a); + float s = 1 - expf((2 * safe_sqrt(_c * sq_gravity / _lift_hover )) * (0.5f * (_lift_hover/(GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1))); + float d = safe_sqrt(_lift_hover / _c); + float flare_distance = ((2 * d / (2 * safe_sqrt(_c * sq_gravity / _lift_hover ))) * (a - logf(fabsf(x)) - (2 * safe_sqrt(_c * sq_gravity / _lift_hover )) * (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) + logf(fabsf(s)))) - d * delta_t_flare; + float delta_h = -flare_distance * cosf(radians(AP_ALPHA_TPP)); + + // Estimate altitude to begin collective pull + _cushion_alt = (-(sink_rate * cosf(radians(AP_ALPHA_TPP))) * _t_tch) * 100.0f; + + // Total delta altitude to ground + _flare_alt_calc = _cushion_alt + delta_h * 100.0f; } #if HAL_LOGGING_ENABLED void AC_Autorotation::Log_Write_Autorotation(void) const { -// @LoggerMessage: AROT -// @Vehicles: Copter -// @Description: Helicopter AutoRotation information -// @Field: TimeUS: Time since system startup -// @Field: P: P-term for headspeed controller response -// @Field: hserr: head speed error; difference between current and desired head speed -// @Field: ColOut: Collective Out -// @Field: FFCol: FF-term for headspeed controller response -// @Field: CRPM: current headspeed RPM -// @Field: SpdF: current forward speed -// @Field: CmdV: desired forward speed -// @Field: p: p-term of velocity response -// @Field: ff: ff-term of velocity response -// @Field: AccO: forward acceleration output -// @Field: AccT: forward acceleration target -// @Field: PitT: pitch target + // @LoggerMessage: AROT + // @Vehicles: Copter + // @Description: Helicopter AutoRotation information + // @Field: TimeUS: Time since system startup + // @Field: hsp: P-term for headspeed controller response + // @Field: hse: head speed error; difference between current and desired head speed + // @Field: co: Collective Out + // @Field: cff: FF-term for headspeed controller response + // @Field: sf: current forward speed + // @Field: dsf: desired forward speed + // @Field: vp: p-term of velocity response + // @Field: vff: ff-term of velocity response + // @Field: az: average z acceleration + // @Field: dvz: Desired Sink Rate + // @Field: h: height above ground //Write to data flash log AP::logger().WriteStreaming("AROT", - "TimeUS,P,hserr,ColOut,FFCol,CRPM,SpdF,CmdV,p,ff,AccO,AccT,PitT", - "Qffffffffffff", - AP_HAL::micros64(), - (double)_p_term_hs, - (double)_head_speed_error, - (double)_collective_out, - (double)_ff_term_hs, - (double)_current_rpm, - (double)_speed_forward, - (double)_cmd_vel, - (double)_vel_p, - (double)_vel_ff, - (double)_accel_out, - (double)_accel_target, - (double)_pitch_target); + "TimeUS,hsp,hse,co,cff,sf,dsf,vp,vff,az,dvz,h", + "Qfffffffffff", + AP_HAL::micros64(), + _p_term_hs, + _head_speed_error, + _collective_out, + _ff_term_hs, + (_speed_forward*0.01f), + (_cmd_vel*0.01f), + _vel_p, + _vel_ff, + _avg_acc_z, + _desired_sink_rate, + (_gnd_hgt*0.01f)); } #endif // HAL_LOGGING_ENABLED -// Initialise forward speed controller -void AC_Autorotation::init_fwd_spd_controller(void) -{ - // Reset I term and acceleration target - _accel_target = 0.0f; - - // Ensure parameter acceleration doesn't exceed hard-coded limit - _accel_max = MIN(_param_accel_max, 60.0f); - - // Reset cmd vel and last accel to sensible values - _cmd_vel = calc_speed_forward(); //(cm/s) - _accel_out_last = _cmd_vel*_param_fwd_k_ff; -} - -// set_dt - sets time delta in seconds for all controllers +// Sets time delta in seconds for all controllers void AC_Autorotation::set_dt(float delta_sec) { _dt = delta_sec; } -// update speed controller +// Update speed controller void AC_Autorotation::update_forward_speed_controller(void) { // Specify forward velocity component and determine delta velocity with respect to time @@ -336,27 +486,27 @@ void AC_Autorotation::update_forward_speed_controller(void) } } - // get p - _vel_p = _p_fw_vel.get_p(_cmd_vel-_speed_forward); + // Get p + _vel_p = _p_fw_vel.get_p(_cmd_vel - _speed_forward); - // get ff - _vel_ff = _cmd_vel*_param_fwd_k_ff; + // Get ff + _vel_ff = _cmd_vel * _param_fwd_k_ff; - //calculate acceleration target based on PI controller + // Calculate acceleration target based on PI controller _accel_target = _vel_ff + _vel_p; - // filter correction acceleration + // Filter correction acceleration _accel_target_filter.set_cutoff_frequency(10.0f); _accel_target_filter.apply(_accel_target, _dt); - //Limits the maximum change in pitch attitude based on acceleration + // Limits the maximum change in pitch attitude based on acceleration if (_accel_target > _accel_out_last + _accel_max) { _accel_target = _accel_out_last + _accel_max; } else if (_accel_target < _accel_out_last - _accel_max) { _accel_target = _accel_out_last - _accel_max; } - //Limiting acceleration based on velocity gained during the previous time step + // Limiting acceleration based on velocity gained during the previous time step if (fabsf(_delta_speed_fwd) > _accel_max * _dt) { _flag_limit_accel = true; } else { @@ -370,9 +520,29 @@ void AC_Autorotation::update_forward_speed_controller(void) } _accel_out_last = _accel_out; - // update angle targets that will be passed to stabilize controller - _pitch_target = accel_to_angle(-_accel_out*0.01) * 100; + if (_gnd_hgt >= _flare_alt_calc * 1.25f) { + _pitch_target = accel_to_angle(-_accel_out * 0.01) * 100; + } else { + _pitch_target = 0.0f; + } +} + +void AC_Autorotation::update_flare_alt(void) +{ + if (!_flare_update_check) { + float delta_v_z = fabsf((_inav.get_velocity_z_up_cms()) * 0.01f + _est_rod); + + if ((_speed_forward >= 0.8f * _param_target_speed) && (delta_v_z <= 2) && (fabsf(_avg_acc_z+GRAVITY_MSS) <= 0.5f)) { + float vel_z = _inav.get_velocity_z_up_cms() * 0.01f; + float spd_fwd = _speed_forward * 0.01f; + _c = (_lift_hover / (sq(vel_z) * 0.5f * SSL_AIR_DENSITY * _disc_area)) * 1.15f; + _c = constrain_float(_c, 0.7f, 1.4f); + calc_flare_alt(vel_z,spd_fwd); + _flare_update_check = true; + gcs().send_text(MAV_SEVERITY_INFO, "Flare_alt_updated=%f C_updated=%f", _flare_alt_calc*0.01f, _c); + } + } } @@ -381,7 +551,134 @@ float AC_Autorotation::calc_speed_forward(void) { auto &ahrs = AP::ahrs(); Vector2f groundspeed_vector = ahrs.groundspeed_vector(); - float speed_forward = (groundspeed_vector.x*ahrs.cos_yaw() + groundspeed_vector.y*ahrs.sin_yaw())* 100; //(c/s) + float speed_forward = (groundspeed_vector.x*ahrs.cos_yaw() + groundspeed_vector.y * ahrs.sin_yaw()) * 100.0; // (cm/s) return speed_forward; } + +void AC_Autorotation::flare_controller(void) +{ + + // Specify forward velocity component and determine delta velocity with respect to time + _speed_forward = calc_speed_forward(); // (cm/s) + _delta_speed_fwd = _speed_forward - _speed_forward_last; // (cm/s) + _speed_forward_last = _speed_forward; // (cm/s) + _desired_speed = linear_interpolate(0.0f, _flare_entry_speed, _gnd_hgt, _cushion_alt, _flare_alt_calc); + + // Get p + _vel_p = _p_fw_vel.get_p(_desired_speed - _speed_forward); + + // Calculate acceleration target based on PI controller + _accel_target = _vel_p; + + // Filter correction acceleration + _accel_target_filter.set_cutoff_frequency(10.0f); + _accel_target_filter.apply(_accel_target, _dt); + + // Limits the maximum change in pitch attitude based on acceleration + if (_accel_target > _accel_out_last + _accel_max) { + _accel_target = _accel_out_last + _accel_max; + } else if (_accel_target < _accel_out_last - _accel_max) { + _accel_target = _accel_out_last - _accel_max; + } + + // Limiting acceleration based on velocity gained during the previous time step + if (fabsf(_delta_speed_fwd) > _accel_max * _dt) { + _flag_limit_accel = true; + } else { + _flag_limit_accel = false; + } + + if ((_flag_limit_accel && fabsf(_accel_target) < fabsf(_accel_out_last)) || !_flag_limit_accel) { + _accel_out = _accel_target; + } else { + _accel_out = _accel_out_last; + } + _accel_out_last = _accel_out; + + // Estimate flare effectiveness + if (_speed_forward <= (0.6 * _flare_entry_speed) && (fabsf(_avg_acc_z+GRAVITY_MSS) <= 0.5f)) { + if (!_flare_complete) { + _flare_complete = true; + gcs().send_text(MAV_SEVERITY_INFO, "Flare_complete"); + } + } + + if (!_flare_complete) { + _pitch_target = atanf(-_accel_out / (GRAVITY_MSS * 100.0f)) * (18000.0f/M_PI); + _pitch_target = constrain_float(_pitch_target, 0.0f, AP_ALPHA_TPP * 100.0f); + } else { + _pitch_target *= 0.9995f; + } +} + + +void AC_Autorotation::touchdown_controller(void) +{ + float _current_sink_rate = _inav.get_velocity_z_up_cms(); + if (_gnd_hgt >= _ground_clearance) { + _desired_sink_rate = linear_interpolate(0.0f, _entry_sink_rate, _gnd_hgt, _ground_clearance, _entry_alt); + } else { + _desired_sink_rate = 0.0f; + } + + // Update forward speed for logging + _speed_forward = calc_speed_forward(); // (cm/s) + + // Check to see if we think the aircraft is on the ground + if(_current_sink_rate < 10.0 && _gnd_hgt <= _ground_clearance && _time_on_ground == 0){ + _time_on_ground = AP_HAL::millis(); + } + + // Use a timer to get confidence that the aircraft is on the ground. + // Note: The landing detector uses the zero thust collective as an indicator for being on the ground. The touch down controller will have + // driven the collective high to cushion the landing so the landing detector will not trip until we drive the collective back to zero thrust and below. + if ((_time_on_ground > 0) && ((AP_HAL::millis() - _time_on_ground) > MIN_TIME_ON_GROUND)) { + // On ground, smoothly lower collective to just bellow zero thrust, to make sure we trip the landing detector + float desired_col = _motors_heli->get_coll_land_min() * 0.95; + _collective_out = _collective_out*0.9 + desired_col*0.1; + + } else { + _collective_out = constrain_value((_p_coll_tch.get_p(_desired_sink_rate - _current_sink_rate))*0.01f + _ff_term_hs, 0.0f, 1.0f); + col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); + _ff_term_hs = col_trim_lpf.apply(_collective_out, _dt); + } + + set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); + + // Smoothly scale the pitch target back to zero (level) + _pitch_target *= 0.95f; +} + + +// Determine if we are above the touchdown height using the descent rate and param values +bool AC_Autorotation::should_begin_touchdown(void) +{ + float vz = _inav.get_velocity_z_up_cms(); + + // We need to be descending for the touch down controller to interpolate the target + // sensibly between the entry of the touch down phase zero. + if (vz >= 0.0) { + return false; + } + + float time_to_ground = (-_gnd_hgt / _inav.get_velocity_z_up_cms()); + + return time_to_ground <= _t_tch.get(); +} + + +void AC_Autorotation::update_avg_acc_z(void) +{ + // Wrap index + if (_index >= 10) { + _index = 0; + } + + _acc_z_sum -= _curr_acc_z[_index]; + _curr_acc_z[_index] = _ahrs.get_accel_ef().z; + _acc_z_sum += _curr_acc_z[_index]; + _index = _index + 1; + + _avg_acc_z = _acc_z_sum / 10.0; +} diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index d5cfa52097b3f1..e44f14a0590040 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -5,40 +5,100 @@ #include #include #include +#include #include #include #include - +#include // Inertial Navigation library class AC_Autorotation { public: //Constructor - AC_Autorotation(); - - //--------Functions-------- - void init_hs_controller(void); // Initialise head speed controller - void init_fwd_spd_controller(void); // Initialise forward speed controller - bool update_hs_glide_controller(float dt); // Update head speed controller - float get_rpm(void) const { return _current_rpm; } // Function just returns the rpm as last read in this library - float get_rpm(bool update_counter); // Function fetches fresh rpm update and continues sensor health monitoring - void set_target_head_speed(float ths) { _target_head_speed = ths; } // Sets the normalised target head speed - void set_col_cutoff_freq(float freq) { _col_cutoff_freq = freq; } // Sets the collective low pass filter cut off frequency + AC_Autorotation(AP_InertialNav& inav, AP_AHRS& ahrs); + + // object initialisation + void init(AP_MotorsHeli* motors, float gnd_clear); + + // Helper to set all necessary variables needed for the entry phase + void init_entry(void); + + // Helper to set all necessary variables needed for the glide phase + void init_glide(float hs_targ); + + // Helper to set all necessary variables needed for the flare phase + void init_flare(float hs_targ); + + // Helper to set all necessary variables needed for the touchdown phase + void init_touchdown(void); + + void initial_flare_estimate(void); + + // Update head speed controller + void update_hs_glide_controller(void); + + // Function fetches fresh rpm update and continues sensor health monitoring + float get_rpm(void); + + // Sets the normalised target head speed + void set_target_head_speed(float ths) { _target_head_speed = ths; } + + // Sets the collective low pass filter cut off frequency + void set_col_cutoff_freq(float freq) { _col_cutoff_freq = freq; } + int16_t get_hs_set_point(void) { return _param_head_speed_set_point; } - float get_col_entry_freq(void) { return _param_col_entry_cutoff_freq; } - float get_col_glide_freq(void) { return _param_col_glide_cutoff_freq; } + float get_bail_time(void) { return _param_bail_time; } + float get_last_collective() const { return _collective_out; } + bool is_enable(void) { return _param_enable; } + void Log_Write_Autorotation(void) const; - void update_forward_speed_controller(void); // Update forward speed controller - void set_desired_fwd_speed(void) { _vel_target = _param_target_speed; } // Overloaded: Set desired speed for forward controller to parameter value - void set_desired_fwd_speed(float speed) { _vel_target = speed; } // Overloaded: Set desired speed to argument value - int32_t get_pitch(void) const { return _pitch_target; } // Get pitch target - float calc_speed_forward(void); // Calculates the forward speed in the horizontal plane + + // Update forward speed controller + void update_forward_speed_controller(void); + + // Get pitch target + int32_t get_pitch(void) const { return _pitch_target; } + + // Calculates the forward speed in the horizontal plane + float calc_speed_forward(void); + + // set the loop time void set_dt(float delta_sec); + // Update the flare controller + void flare_controller(void); + + // Update the touchdown controller + void touchdown_controller(void); + + void set_ground_distance(float gnd_dist) { _gnd_hgt = (float)gnd_dist; } + + void get_entry_speed(void); + + void set_entry_sink_rate (float sink_rate) { _entry_sink_rate = sink_rate; } + + void set_entry_alt (float entry_alt) { _entry_alt = entry_alt; } + + bool above_flare_height(void) const { return _gnd_hgt > _flare_alt_calc; } + + void update_hover_autorotation_controller(); + + // update rolling average z-accel filter + void update_avg_acc_z(void); + + void update_flare_alt(void); + + void calc_flare_alt(float sink_rate, float fwd_speed); + + // Estimate the time to impact and compare it with the touchdown time param + bool should_begin_touchdown(void); + + bool use_stabilise_controls(void) const { return _options.get() & int32_t(OPTION::STABILISE_CONTROLS); } + // User Settable Parameters static const struct AP_Param::GroupInfo var_info[]; @@ -49,26 +109,44 @@ class AC_Autorotation float _collective_out; float _head_speed_error; // Error between target head speed and current head speed. Normalised by head speed set point RPM. float _col_cutoff_freq; // Lowpass filter cutoff frequency (Hz) for collective. - uint8_t _unhealthy_rpm_counter; // Counter used to track RPM sensor unhealthy signal. - uint8_t _healthy_rpm_counter; // Counter used to track RPM sensor healthy signal. + uint16_t _unhealthy_rpm_counter; // Counter used to track RPM sensor unhealthy signal. float _target_head_speed; // Normalised target head speed. Normalised by head speed set point RPM. float _p_term_hs; // Proportional contribution to collective setting. float _ff_term_hs; // Following trim feed forward contribution to collective setting. - float _vel_target; // Forward velocity target. float _pitch_target; // Pitch angle target. float _accel_max; // Maximum acceleration limit. - int16_t _speed_forward_last; // The forward speed calculated in the previous cycle. + int16_t _speed_forward_last; // The forward speed calculated in the previous cycle. bool _flag_limit_accel; // Maximum acceleration limit reached flag. float _accel_out_last; // Acceleration value used to calculate pitch target in previous cycle. float _cmd_vel; // Command velocity, used to get PID values for acceleration calculation. float _accel_target; // Acceleration target, calculated from PID. float _delta_speed_fwd; // Change in forward speed between computation cycles. float _dt; // Time step. - int16_t _speed_forward; // Measured forward speed. + int16_t _speed_forward; // Measured forward speed. float _vel_p; // Forward velocity P term. float _vel_ff; // Forward velocity Feed Forward term. float _accel_out; // Acceleration value used to calculate pitch target. + float _entry_sink_rate; // Descent rate at beginning of touvhdown collective pull + float _entry_alt; // Altitude at beginning of touchdown coll pull + float _flare_entry_speed; // Traslational velocity at beginning of flare maneuver + float _desired_speed; // Desired traslational velocity during flare + float _desired_sink_rate; // Desired vertical velocity during touchdown + float _ground_clearance; // Sensor offset distance + float _gnd_hgt; // Height above ground, passed down from copter, can be from lidar or terrain + float _avg_acc_z; // Averaged vertical acceleration + float _acc_z_sum; // Sum of vertical acceleration samples + int16_t _index; // Index for vertical acceleration rolling average + float _curr_acc_z[10]; // Array for storing vertical acceleration samples + float _flare_alt_calc; // Calculated flare altitude + float _lift_hover; // Main rotor thrust in hover condition + float _c; // Main rotor drag coefficient + float _cushion_alt; // Altitude for touchdown collective pull + float _disc_area; // Main rotor disc area + float _est_rod; // Estimated rate of descent (vertical autorotation) + bool _flare_complete; // Flare completed + bool _flare_update_check; // Check for flare altitude updating + uint32_t _time_on_ground; // Time elapsed after touch down LowPassFilterFloat _accel_target_filter; // acceleration target filter @@ -76,24 +154,34 @@ class AC_Autorotation AP_Int8 _param_enable; AC_P _p_hs; AC_P _p_fw_vel; - AP_Int16 _param_head_speed_set_point; - AP_Int16 _param_target_speed; + AC_P _p_coll_tch; AP_Float _param_col_entry_cutoff_freq; AP_Float _param_col_glide_cutoff_freq; + AP_Float _param_col_touchdown_cutoff_freq; AP_Int16 _param_accel_max; AP_Float _param_bail_time; AP_Int8 _param_rpm_instance; AP_Float _param_fwd_k_ff; + AP_Int16 _param_target_speed; + AP_Int16 _param_head_speed_set_point; + AP_Float _param_solidity; + AP_Float _param_diameter; + AP_Float _t_tch; + AP_Int32 _options; - //--------Internal Flags-------- - struct controller_flags { - bool bad_rpm : 1; - bool bad_rpm_warning : 1; - } _flags; + enum class OPTION { + STABILISE_CONTROLS=(1<<0), + }; //--------Internal Functions-------- + // set the collective in the motors library void set_collective(float _collective_filter_cutoff) const; // low pass filter for collective trim LowPassFilterFloat col_trim_lpf; + + //--------References to Other Libraries-------- + AP_InertialNav& _inav; + AP_AHRS& _ahrs; + AP_MotorsHeli* _motors_heli; }; diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index fc93264ed5d7f9..d84b463a20064b 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -613,3 +613,16 @@ AP_MotorsHeli_RSC::RotorControlState AP_MotorsHeli::get_rotor_control_state() co // Should be unreachable, but needed to keep the compiler happy return AP_MotorsHeli_RSC::RotorControlState::STOP; } + +// Return collective hover position as an angle in deg +float AP_MotorsHeli::get_hover_coll_ang(void) +{ + return _collective_min_deg.get() + (_collective_max_deg.get() - _collective_min_deg.get()) * _collective_hover.get(); +} + +// Helper function to calculate the normalised collective position given a desired blade pitch angle (deg) +float AP_MotorsHeli::calc_coll_from_ang(float col_ang_deg) +{ + float col_norm = col_ang_deg / MAX((_collective_max_deg.get() - _collective_min_deg.get()), 1.0); + return constrain_float(col_norm, 0.0, 1.0); +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index f2a8ce6100400b..7b2cb11db4deec 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -84,6 +84,9 @@ class AP_MotorsHeli : public AP_Motors { // get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1) float get_rsc_setpoint() const { return _main_rotor._rsc_setpoint.get() * 0.01f; } + //return governor rpm setpoint + float get_rpm_setpoint() const {return _main_rotor.get_governor_setpoint();} + // arot_man_enabled - gets contents of manual_autorotation_enabled parameter bool arot_man_enabled() const { return (_main_rotor._rsc_arot_man_enable.get() == 1) ? true : false; } @@ -145,6 +148,22 @@ class AP_MotorsHeli : public AP_Motors { //return zero lift collective position float get_coll_mid() const { return _collective_zero_thrust_pct; } + //return landing collective position + float get_coll_land_min()const { return _collective_land_min_pct;} + + //return collective hover + float get_coll_hover() const { return _collective_hover; } + + float get_coll_max_pitch() const { return _collective_max_deg;} + + float get_coll_min_pitch() const { return _collective_min_deg;} + + // Return collective hover position as an angle in deg + float get_hover_coll_ang(void); + + // Helper function to calculate the normalised collective position given a desired blade pitch angle (deg) + float calc_coll_from_ang(float col_ang_deg); + // enum for heli optional features enum class HeliOption { USE_LEAKY_I = (1<<0), // 1 @@ -278,6 +297,7 @@ class AP_MotorsHeli : public AP_Motors { // internal variables float _collective_zero_thrust_pct; // collective zero thrutst parameter value converted to 0 ~ 1 range float _collective_land_min_pct; // collective land min parameter value converted to 0 ~ 1 range + float _collective_hover_rad; uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup motor_frame_type _frame_type; diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 276728ca4d1dc3..2dfe073569ab1d 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -87,6 +87,7 @@ class AP_MotorsHeli_RSC { // functions for autothrottle, throttle curve, governor, idle speed, output to servo void set_governor_output(float governor_output) {_governor_output = governor_output; } float get_governor_output() const { return _governor_output; } + float get_governor_setpoint() const { return _governor_rpm; } void governor_reset(); float get_control_output() const { return _control_output; } void set_idle_output(float idle_output) { _idle_output.set(idle_output); } @@ -129,7 +130,7 @@ class AP_MotorsHeli_RSC { uint32_t get_output_mask() const; // rotor_speed_above_critical - return true if rotor speed is above that critical for flight - bool rotor_speed_above_critical(void) const { return get_rotor_speed() > get_critical_speed(); } + bool rotor_speed_above_critical(void) const { return get_rotor_speed() >= get_critical_speed(); } // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[];