Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Heli: simplify autorotation mode change and support RSC autorotation state

This commit is contained in:
Gone4Dirt 2024-08-04 18:32:47 +01:00 committed by Peter Barker
parent 431cc25dca
commit 075ce596d2
6 changed files with 49 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -390,7 +390,6 @@ private:
// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
Mode *flightmode;
Mode::Number prev_control_mode;
RCMapper rcmap;

34
ArduCopter/heli.cpp
View File

@ -189,29 +189,25 @@ void Copter::heli_update_rotor_speed_targets()
// to autorotation flight mode if manual collective is not being used.
void Copter::heli_update_autorotation()
{
// check if flying and interlock disengaged
if (!ap.land_complete && !motors->get_interlock()) {
bool in_autorotation_mode = false;
#if MODE_AUTOROTATE_ENABLED
if (g2.arot.is_enable()) {
if (!flightmode->has_manual_throttle()) {
// set autonomous autorotation flight mode
set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START);
}
// set flag to facilitate both auto and manual autorotations
motors->set_in_autorotation(true);
motors->set_enable_bailout(true);
}
in_autorotation_mode = flightmode == &mode_autorotate;
#endif
if (flightmode->has_manual_throttle() && motors->arot_man_enabled()) {
// set flag to facilitate both auto and manual autorotations
motors->set_in_autorotation(true);
motors->set_enable_bailout(true);
}
} else {
motors->set_in_autorotation(false);
motors->set_enable_bailout(false);
// If we have landed then we do not want to be in autorotation and we do not want to via the bailout state
if (ap.land_complete || ap.land_complete_maybe) {
motors->force_deactivate_autorotation();
return;
}
// if we got this far we are flying, check for conditions to set autorotation state
if (!motors->get_interlock() && (flightmode->has_manual_throttle() || in_autorotation_mode)) {
// set state in motors to facilitate manual and assisted autorotations
motors->set_autorotation_active(true);
} else {
// deactivate the autorotation state via the bailout case
motors->set_autorotation_active(false);
}
}
// update collective low flag. Use a debounce time of 400 milliseconds.

16
ArduCopter/mode.cpp
View File

@ -292,21 +292,10 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter a non-manual throttle mode if the
// rotor runup is not complete
if (!ignore_checks && !new_flightmode->has_manual_throttle() &&
(motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) {
#if MODE_AUTOROTATE_ENABLED
//if the mode being exited is the autorotation mode allow mode change despite rotor not being at
//full speed. This will reduce altitude loss on bail-outs back to non-manual throttle modes
bool in_autorotation_check = (flightmode != &mode_autorotate || new_flightmode != &mode_autorotate);
#else
bool in_autorotation_check = false;
#endif
if (!in_autorotation_check) {
if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()) {
mode_change_failed(new_flightmode, "runup not complete");
return false;
}
}
#endif
#if FRAME_CONFIG != HELI_FRAME
@ -369,9 +358,6 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
// perform any cleanup required by previous flight mode
exit_mode(flightmode, new_flightmode);
// store previous flight mode (only used by tradeheli's autorotation)
prev_control_mode = flightmode->mode_number();
// update flight mode
flightmode = new_flightmode;
control_mode_reason = reason;

8
ArduCopter/mode.h
View File

@ -2015,17 +2015,13 @@ private:
int32_t _pitch_target; // Target pitch attitude to pass to attitude controller
uint32_t _entry_time_start_ms; // Time remaining until entry phase moves on to glide phase
float _hs_decay; // The head accerleration during the entry phase
float _bail_time; // Timer for exiting the bail out phase (s)
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
enum class Autorotation_Phase {
ENTRY,
SS_GLIDE,
FLARE,
TOUCH_DOWN,
BAIL_OUT } phase_switch;
LANDED } phase_switch;
enum class Navigation_Decision {
USER_CONTROL_STABILISED,
@ -2039,10 +2035,10 @@ private:
bool ss_glide_initial : 1;
bool flare_initial : 1;
bool touch_down_initial : 1;
bool landed_initial : 1;
bool straight_ahead_initial : 1;
bool level_initial : 1;
bool break_initial : 1;
bool bail_out_initial : 1;
bool bad_rpm : 1;
} _flags;

109
ArduCopter/mode_autorotate.cpp
View File

@ -14,8 +14,8 @@
#if MODE_AUTOROTATE_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 AUTOROTATION_MIN_MOVING_SPEED 100.0 // (cm/s) minimum speed used for "is moving" check
bool ModeAutorotate::init(bool ignore_checks)
{
@ -24,15 +24,16 @@ bool ModeAutorotate::init(bool ignore_checks)
return false;
#endif
// Check that mode is enabled
// 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_INFO, "Autorot Mode Not Enabled");
gcs().send_text(MAV_SEVERITY_WARNING, "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");
// 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;
}
@ -52,10 +53,10 @@ bool ModeAutorotate::init(bool ignore_checks)
_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;
_flags.bail_out_initial = true;
_msg_flags.bad_rpm = true;
// Setting default starting switches
@ -74,20 +75,9 @@ bool ModeAutorotate::init(bool ignore_checks)
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
//----------------------------------------------------------------
// State machine logic
//----------------------------------------------------------------
@ -97,12 +87,22 @@ void ModeAutorotate::run()
// 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);
}
@ -199,79 +199,22 @@ void ModeAutorotate::run()
{
break;
}
case Autorotation_Phase::BAIL_OUT:
case Autorotation_Phase::LANDED:
{
if (_flags.bail_out_initial == true) {
// Functions and settings to be done once are done here.
// 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, "Bailing Out of Autorotation");
gcs().send_text(MAV_SEVERITY_INFO, "Landed");
#endif
// 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 bail out start time
_bail_time_start_ms = 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_z_controller(g2.arot.get_last_collective());
_flags.landed_initial = false;
}
// 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_initial = false;
}
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;
}
// Set position controller
pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z);
// Update controllers
pos_control->update_z_controller();
if ((now - _bail_time_start_ms)/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);
}
}
// 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:

2
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -540,7 +540,7 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang
update_althold_lean_angle_max(throttle_in);
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (apply_angle_boost && !((AP_MotorsHeli&)_motors).get_in_autorotation()) {
if (apply_angle_boost && !((AP_MotorsHeli&)_motors).in_autorotation()) {
// Apply angle boost
throttle_in = get_throttle_boosted(throttle_in);
} else {