mirror of https://github.com/ArduPilot/ardupilot
326 lines
12 KiB
C++
326 lines
12 KiB
C++
/* -----------------------------------------------------------------------------------------
|
|
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
|