Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 15:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Autorotation mode restructure and tidy

Browse Source
This commit is contained in:
Gone4Dirt 2024-09-10 12:19:03 +01:00 committed by Andrew Tridgell
parent 575af5e398
commit 00e5f4001f
5 changed files with 73 additions and 230 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/Copter.cpp
View File

@ -716,12 +716,6 @@ void Copter::twentyfive_hz_logging()
AP::ins().Write_IMU();
}
#if MODE_AUTOROTATE_ENABLED
if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
//update autorotation log
g2.arot.Log_Write_Autorotation();
}
#endif
#if HAL_GYROFFT_ENABLED
if (should_log(MASK_LOG_FTN_FAST)) {
gyro_fft.write_log_messages();

4
ArduCopter/Copter.h
View File

@ -164,6 +164,10 @@
#error AP_OAPathPlanner relies on AP_FENCE_ENABLED which is disabled
#endif
#if MODE_AUTOROTATE_ENABLED && !AP_RPM_ENABLED
#error AC_Autorotation relies on AP_RPM_ENABLED which is disabled
#endif
#if HAL_ADSB_ENABLED
#include "avoidance_adsb.h"
#endif

2
ArduCopter/Parameters.cpp
View File

@ -1293,7 +1293,7 @@ ParametersG2::ParametersG2(void) :
,mode_systemid_ptr(&copter.mode_systemid)
#endif
#if MODE_AUTOROTATE_ENABLED
,arot()
,arot(copter.motors, copter.attitude_control)
#endif
#if MODE_ZIGZAG_ENABLED
,mode_zigzag_ptr(&copter.mode_zigzag)

47
ArduCopter/mode.h
View File

@ -2033,46 +2033,21 @@ protected:
private:
// --- Internal variables ---
float _initial_rpm; // Head speed recorded at initiation of flight mode (RPM)
float _target_head_speed; // The terget head main rotor head speed. Normalised by main rotor set point
float _desired_v_z; // Desired vertical
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
uint32_t _entry_time_start_ms; // time remaining until entry phase moves on to glide phase
uint32_t _last_logged_ms; // used for timing slow rate autorotation log
enum class Autorotation_Phase {
enum class Phase {
ENTRY_INIT,
ENTRY,
SS_GLIDE,
GLIDE_INIT,
GLIDE,
FLARE_INIT,
FLARE,
TOUCH_DOWN_INIT,
TOUCH_DOWN,
LANDED } 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 landed_initial : 1;
bool straight_ahead_initial : 1;
bool level_initial : 1;
bool break_initial : 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
LANDED_INIT,
LANDED,
} current_phase;
};
#endif

244
ArduCopter/mode_autorotate.cpp
View File

@ -9,14 +9,8 @@
#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
@ -27,77 +21,55 @@ bool ModeAutorotate::init(bool ignore_checks)
// 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.enabled()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Autorot Mode Not Enabled");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AROT: 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");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AROT: 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();
// Initialise controller
g2.arot.init();
// 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;
// Setting default starting state
current_phase = Phase::ENTRY_INIT;
// 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;
// reset logging timer
_last_logged_ms = 0;
return true;
}
void ModeAutorotate::run()
{
// Current time
uint32_t now = millis(); //milliseconds
const uint32_t now_ms = millis();
// 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);
//----------------------------------------------------------------
// 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.
// 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;
}
if (current_phase < Phase::GLIDE_INIT && ((now_ms - _entry_time_start_ms) > g2.arot.entry_time_ms)) {
// Flight phase can be progressed to steady state glide
current_phase = Phase::GLIDE_INIT;
}
// 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 believe we have landed. We need the landed state to zero all
// controls and make sure that the copter landing detector will trip
if (current_phase < Phase::LANDED && g2.arot.check_landed()) {
current_phase = Phase::LANDED_INIT;
}
// Check if we are bailing out and need to re-set the spool state
@ -105,162 +77,60 @@ void ModeAutorotate::run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
}
// Get norm input from yaw channel
const float pilot_norm_input = channel_yaw->norm_input_dz();
//----------------------------------------------------------------
// State machine actions
//----------------------------------------------------------------
switch (phase_switch) {
switch (current_phase) {
case Autorotation_Phase::ENTRY:
{
case Phase::ENTRY_INIT:
// 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
g2.arot.init_entry();
current_phase = Phase::ENTRY;
FALLTHROUGH;
case Phase::ENTRY:
// Smoothly transition the collective to enter autorotation regime and begin forward speed control
g2.arot.run_entry(pilot_norm_input);
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
case Phase::GLIDE_INIT:
// Glide phase functions to be run only once
g2.arot.init_glide();
current_phase = Phase::GLIDE;
FALLTHROUGH;
case Phase::GLIDE:
// Maintain head speed and forward speed as we glide to the ground
g2.arot.run_glide(pilot_norm_input);
break;
}
case Autorotation_Phase::FLARE:
case Autorotation_Phase::TOUCH_DOWN:
{
case Phase::FLARE_INIT:
case Phase::FLARE:
case Phase::TOUCH_DOWN_INIT:
case 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;
case Phase::LANDED_INIT:
// Landed phase functions to be run only once
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AROT: Landed");
current_phase = Phase::LANDED;
FALLTHROUGH;
case Phase::LANDED:
// Don't allow controller to continually ask for more pitch to build speed when we are on the ground, decay to zero smoothly
g2.arot.run_landed();
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);
// Slow rate (25 Hz) logging for the mode
if (now_ms - _last_logged_ms > 40U) {
g2.arot.log_write_autorotation();
_last_logged_ms = now_ms;
}
} // 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