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

Copter: fixed use of timestamps in autorotate 

fixes wrap handling
This commit is contained in:
Andrew Tridgell 2020-04-19 11:32:01 +10:00
parent 02aebfcd6f
commit 4c2cf2139e
2 changed files with 8 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/mode.h
View File

@ -1456,11 +1456,10 @@ private:
float _target_head_speed; // The terget head main rotor head speed. Normalised by main rotor set point float _target_head_speed; // The terget head main rotor head speed. Normalised by main rotor set point
float _desired_v_z; // Desired vertical float _desired_v_z; // Desired vertical
int32_t _pitch_target; // Target pitch attitude to pass to attitude controller int32_t _pitch_target; // Target pitch attitude to pass to attitude controller
float now; // Current time in millis uint32_t _entry_time_start_ms; // Time remaining until entry phase moves on to glide phase
float _entry_time_start; // Time remaining until entry phase moves on to glide phase
float _hs_decay; // The head accerleration during the entry phase float _hs_decay; // The head accerleration during the entry phase
float _bail_time; // Timer for exiting the bail out phase (s) float _bail_time; // Timer for exiting the bail out phase (s)
float _bail_time_start; // Time at start of bail out 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_climb_rate_adjust;// Target vertical acceleration used during bail out phase
float _target_pitch_adjust; // Target pitch rate used during bail out phase float _target_pitch_adjust; // Target pitch rate used during bail out phase

12
ArduCopter/mode_autorotate.cpp
View File

@ -62,7 +62,7 @@ bool ModeAutorotate::init(bool ignore_checks)
phase_switch = Autorotation_Phase::ENTRY; phase_switch = Autorotation_Phase::ENTRY;
// Set entry timer // Set entry timer
_entry_time_start = millis(); _entry_time_start_ms = millis();
// The decay rate to reduce the head speed from the current to the target // 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; _hs_decay = ((_initial_rpm/g2.arot.get_hs_set_point()) - HEAD_SPEED_TARGET_RATIO) / AUTOROTATE_ENTRY_TIME;
@ -83,7 +83,7 @@ void ModeAutorotate::run()
} }
// Current time // Current time
now = millis(); //milliseconds uint32_t now = millis(); //milliseconds
// Initialise internal variables // Initialise internal variables
float curr_vel_z = inertial_nav.get_velocity().z; // Current vertical descent float curr_vel_z = inertial_nav.get_velocity().z; // Current vertical descent
@ -98,7 +98,7 @@ void ModeAutorotate::run()
// Timer from entry phase to progress to glide phase // Timer from entry phase to progress to glide phase
if (phase_switch == Autorotation_Phase::ENTRY){ if (phase_switch == Autorotation_Phase::ENTRY){
if ((now - _entry_time_start)/1000.0f > AUTOROTATE_ENTRY_TIME) { if ((now - _entry_time_start_ms)/1000.0f > AUTOROTATE_ENTRY_TIME) {
// Flight phase can be progressed to steady state glide // Flight phase can be progressed to steady state glide
phase_switch = Autorotation_Phase::SS_GLIDE; phase_switch = Autorotation_Phase::SS_GLIDE;
} }
@ -214,7 +214,7 @@ void ModeAutorotate::run()
_bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f); _bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f);
// Set bail out start time // Set bail out start time
_bail_time_start = now; _bail_time_start_ms = now;
// Set initial target vertical speed // Set initial target vertical speed
_desired_v_z = curr_vel_z; _desired_v_z = curr_vel_z;
@ -248,7 +248,7 @@ void ModeAutorotate::run()
_flags.bail_out_initial = 0; _flags.bail_out_initial = 0;
} }
if ((now - _bail_time_start)/1000.0f >= BAILOUT_MOTOR_RAMP_TIME) { 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 // Update desired vertical speed and pitch target after the bailout motor ramp timer has completed
_desired_v_z -= _target_climb_rate_adjust*G_Dt; _desired_v_z -= _target_climb_rate_adjust*G_Dt;
_pitch_target -= _target_pitch_adjust*G_Dt; _pitch_target -= _target_pitch_adjust*G_Dt;
@ -259,7 +259,7 @@ void ModeAutorotate::run()
// Update controllers // Update controllers
pos_control->update_z_controller(); pos_control->update_z_controller();
if ((now - _bail_time_start)/1000.0f >= _bail_time) { 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 // 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. // from continuing mission and potentially flying further away after a power failure.
if (copter.prev_control_mode == Mode::Number::AUTO) { if (copter.prev_control_mode == Mode::Number::AUTO) {