Gone4Dirt
5 years ago
committed by
Randy Mackay
12 changed files with 542 additions and 14 deletions
@ -0,0 +1,325 @@ |
|||||||
|
/* -----------------------------------------------------------------------------------------
|
||||||
|
This is currently a SITL only function until the project is complete. |
||||||
|
To trial this in SITL you will need to use Real Flight 8. |
||||||
|
Instructions for how to set this up in SITL can be found here: |
||||||
|
https://discuss.ardupilot.org/t/autonomous-autorotation-gsoc-project-blog/42139
|
||||||
|
-----------------------------------------------------------------------------------------*/ |
||||||
|
|
||||||
|
#include "Copter.h" |
||||||
|
#include <AC_Autorotation/AC_Autorotation.h> |
||||||
|
#include "mode.h" |
||||||
|
|
||||||
|
#include <utility> |
||||||
|
|
||||||
|
#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: -)
|
||||||
|
|
||||||
|
bool ModeAutorotate::init(bool ignore_checks) |
||||||
|
{ |
||||||
|
#if FRAME_CONFIG != HELI_FRAME |
||||||
|
// Only allow trad heli to use autorotation mode
|
||||||
|
return false; |
||||||
|
#endif |
||||||
|
|
||||||
|
// Check that mode is enabled
|
||||||
|
if (!g2.arot.is_enable()) { |
||||||
|
gcs().send_text(MAV_SEVERITY_INFO, "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"); |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
// Initialise controllers
|
||||||
|
// This must be done before RPM value is fetched
|
||||||
|
g2.arot.init_hs_controller(); |
||||||
|
g2.arot.init_fwd_spd_controller(); |
||||||
|
|
||||||
|
// Retrive 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 inial flags to on
|
||||||
|
_flags.entry_initial = 1; |
||||||
|
_flags.ss_glide_initial = 1; |
||||||
|
_flags.flare_initial = 1; |
||||||
|
_flags.touch_down_initial = 1; |
||||||
|
_flags.level_initial = 1; |
||||||
|
_flags.break_initial = 1; |
||||||
|
_flags.straight_ahead_initial = 1; |
||||||
|
_flags.bail_out_initial = 1; |
||||||
|
_msg_flags.bad_rpm = true; |
||||||
|
|
||||||
|
// Setting default starting switches
|
||||||
|
phase_switch = Autorotation_Phase::ENTRY; |
||||||
|
|
||||||
|
// Set entry timer
|
||||||
|
_entry_time_start = 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; |
||||||
|
|
||||||
|
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
|
||||||
|
now = millis(); //milliseconds
|
||||||
|
|
||||||
|
// Initialise internal variables
|
||||||
|
float curr_vel_z = inertial_nav.get_velocity().z; // Current vertical descent
|
||||||
|
|
||||||
|
//----------------------------------------------------------------
|
||||||
|
// State machine logic
|
||||||
|
//----------------------------------------------------------------
|
||||||
|
|
||||||
|
// 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)/1000.0f > AUTOROTATE_ENTRY_TIME) { |
||||||
|
// Flight phase can be progressed to steady state glide
|
||||||
|
phase_switch = Autorotation_Phase::SS_GLIDE; |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
//----------------------------------------------------------------
|
||||||
|
// State machine actions
|
||||||
|
//----------------------------------------------------------------
|
||||||
|
switch (phase_switch) { |
||||||
|
|
||||||
|
case Autorotation_Phase::ENTRY: |
||||||
|
{ |
||||||
|
// Entry phase functions to be run only once
|
||||||
|
if (_flags.entry_initial == 1) { |
||||||
|
|
||||||
|
#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 = 0; |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
// 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
|
||||||
|
|
||||||
|
break; |
||||||
|
} |
||||||
|
|
||||||
|
case Autorotation_Phase::SS_GLIDE: |
||||||
|
{ |
||||||
|
// Steady state glide functions to be run only once
|
||||||
|
if (_flags.ss_glide_initial == 1) { |
||||||
|
|
||||||
|
#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 incase hs hasent reached target for glide
|
||||||
|
g2.arot.set_target_head_speed(_target_head_speed); |
||||||
|
|
||||||
|
// Prevent running the initial glide functions again
|
||||||
|
_flags.ss_glide_initial = 0; |
||||||
|
} |
||||||
|
|
||||||
|
// 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
|
||||||
|
|
||||||
|
break; |
||||||
|
} |
||||||
|
|
||||||
|
case Autorotation_Phase::FLARE: |
||||||
|
case Autorotation_Phase::TOUCH_DOWN: |
||||||
|
{ |
||||||
|
break; |
||||||
|
} |
||||||
|
|
||||||
|
case Autorotation_Phase::BAIL_OUT: |
||||||
|
{ |
||||||
|
if (_flags.bail_out_initial == 1) { |
||||||
|
// Functions and settings to be done once are done here.
|
||||||
|
|
||||||
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
||||||
|
gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation"); |
||||||
|
#endif |
||||||
|
|
||||||
|
// Set bail out timer remaining equal to the paramter 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 = 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_alt_hold_controllers(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; |
||||||
|
|
||||||
|
// Set speed limit
|
||||||
|
pos_control->set_max_speed_z(curr_vel_z, pilot_spd_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 decent 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 acceleration limit
|
||||||
|
pos_control->set_max_accel_z(abs(_target_climb_rate_adjust)); |
||||||
|
|
||||||
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); |
||||||
|
|
||||||
|
_flags.bail_out_initial = 0; |
||||||
|
} |
||||||
|
|
||||||
|
if ((now - _bail_time_start)/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_alt_target_from_climb_rate(_desired_v_z, G_Dt, false); |
||||||
|
|
||||||
|
// Update controllers
|
||||||
|
pos_control->update_z_controller(); |
||||||
|
|
||||||
|
if ((now - _bail_time_start)/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; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
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->get_control_in()); |
||||||
|
|
||||||
|
// 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); |
||||||
|
} |
||||||
|
|
||||||
|
} // 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 |
||||||
Loading…
Reference in new issue