mirror of https://github.com/ArduPilot/ardupilot
267 lines
9.6 KiB
C++
267 lines
9.6 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
|
|
|
|
#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
|
|
// Only allow trad heli to use autorotation mode
|
|
return false;
|
|
#endif
|
|
|
|
// 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.is_enable()) {
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Autorot 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");
|
|
return false;
|
|
}
|
|
|
|
// Initialise controllers
|
|
// This must be done before RPM value is fetched
|
|
g2.arot.init_hs_controller();
|
|
g2.arot.init_fwd_spd_controller();
|
|
|
|
// 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;
|
|
|
|
// 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;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
void ModeAutorotate::run()
|
|
{
|
|
// Current time
|
|
uint32_t now = millis(); //milliseconds
|
|
|
|
//----------------------------------------------------------------
|
|
// 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_ms)/1000.0f > AUTOROTATE_ENTRY_TIME) {
|
|
// Flight phase can be progressed to steady state glide
|
|
phase_switch = Autorotation_Phase::SS_GLIDE;
|
|
}
|
|
}
|
|
|
|
// 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 are bailing out and need to re-set the spool state
|
|
if (motors->autorotation_bailout()) {
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------
|
|
// State machine actions
|
|
//----------------------------------------------------------------
|
|
switch (phase_switch) {
|
|
|
|
case Autorotation_Phase::ENTRY:
|
|
{
|
|
// 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
|
|
|
|
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
|
|
|
|
break;
|
|
}
|
|
|
|
case Autorotation_Phase::FLARE:
|
|
case Autorotation_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;
|
|
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);
|
|
}
|
|
|
|
} // 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
|