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

Copter: clean up flip state enumerations

Browse Source 
... including a redundant definition of the states...
This commit is contained in:
Peter Barker 2019-04-08 13:20:47 +10:00 committed by Randy Mackay
parent fef42c6219
commit 85be2b0c8b
3 changed files with 26 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/defines.h
View File

@ -176,16 +176,6 @@ enum SmartRTLState {
SmartRTL_Land
};
// Flip states
enum FlipState {
Flip_Start,
Flip_Roll,
Flip_Pitch_A,
Flip_Pitch_B,
Flip_Recover,
Flip_Abandon
};
enum LandStateType {
LandStateType_FlyToLocation = 0,
LandStateType_Descending = 1

14
ArduCopter/mode.h
View File

@ -616,13 +616,13 @@ private:
// Flip
Vector3f orig_attitude; // original vehicle attitude before flip
enum FlipState {
Flip_Start,
Flip_Roll,
Flip_Pitch_A,
Flip_Pitch_B,
Flip_Recover,
Flip_Abandon
enum class FlipState : uint8_t {
Start,
Roll,
Pitch_A,
Pitch_B,
Recover,
Abandon
};
FlipState _state; // current state of flip
control_mode_t orig_control_mode; // flight mode when flip was initated

38
ArduCopter/mode_flip.cpp
View File

@ -15,13 +15,13 @@
* Pilot may manually exit flip by switching off ch7/ch8 or by moving roll stick to >40deg left or right
*
* State machine approach:
* Flip_Start (while copter is leaning <45deg) : roll right at 400deg/sec, increase throttle
* Flip_Roll (while copter is between +45deg ~ -90) : roll right at 400deg/sec, reduce throttle
* Flip_Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude
* FlipState::Start (while copter is leaning <45deg) : roll right at 400deg/sec, increase throttle
* FlipState::Roll (while copter is between +45deg ~ -90) : roll right at 400deg/sec, reduce throttle
* FlipState::Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude
*/
#define FLIP_THR_INC 0.20f // throttle increase during Flip_Start stage (under 45deg lean angle)
#define FLIP_THR_DEC 0.24f // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll)
#define FLIP_THR_INC 0.20f // throttle increase during FlipState::Start stage (under 45deg lean angle)
#define FLIP_THR_DEC 0.24f // throttle decrease during FlipState::Roll stage (between 45deg ~ -90deg roll)
#define FLIP_ROTATION_RATE 40000 // rotation rate request in centi-degrees / sec (i.e. 400 deg/sec)
#define FLIP_TIMEOUT_MS 2500 // timeout after 2.5sec. Vehicle will switch back to original flight mode
#define FLIP_RECOVERY_ANGLE 500 // consider successful recovery when roll is back within 5 degrees of original
@ -62,7 +62,7 @@ bool Copter::ModeFlip::init(bool ignore_checks)
orig_control_mode = copter.control_mode;
// initialise state
_state = Flip_Start;
_state = FlipState::Start;
start_time_ms = millis();
roll_dir = pitch_dir = 0;
@ -81,7 +81,7 @@ bool Copter::ModeFlip::init(bool ignore_checks)
// log start of flip
Log_Write_Event(DATA_FLIP_START);
// capture current attitude which will be used during the Flip_Recovery stage
// capture current attitude which will be used during the FlipState::Recovery stage
const float angle_max = copter.aparm.angle_max;
orig_attitude.x = constrain_float(ahrs.roll_sensor, -angle_max, angle_max);
orig_attitude.y = constrain_float(ahrs.pitch_sensor, -angle_max, angle_max);
@ -96,7 +96,7 @@ void Copter::ModeFlip::run()
{
// if pilot inputs roll > 40deg or timeout occurs abandon flip
if (!motors->armed() || (abs(channel_roll->get_control_in()) >= 4000) || (abs(channel_pitch->get_control_in()) >= 4000) || ((millis() - start_time_ms) > FLIP_TIMEOUT_MS)) {
_state = Flip_Abandon;
_state = FlipState::Abandon;
}
// get pilot's desired throttle
@ -118,7 +118,7 @@ void Copter::ModeFlip::run()
// state machine
switch (_state) {
case Flip_Start:
case FlipState::Start:
// under 45 degrees request 400deg/sec roll or pitch
attitude_control->input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * roll_dir, FLIP_ROTATION_RATE * pitch_dir, 0.0);
@ -129,15 +129,15 @@ void Copter::ModeFlip::run()
if (flip_angle >= 4500) {
if (roll_dir != 0) {
// we are rolling
_state = Flip_Roll;
_state = FlipState::Roll;
} else {
// we are pitching
_state = Flip_Pitch_A;
_state = FlipState::Pitch_A;
}
}
break;
case Flip_Roll:
case FlipState::Roll:
// between 45deg ~ -90deg request 400deg/sec roll
attitude_control->input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * roll_dir, 0.0, 0.0);
// decrease throttle
@ -145,11 +145,11 @@ void Copter::ModeFlip::run()
// beyond -90deg move on to recovery
if ((flip_angle < 4500) && (flip_angle > -9000)) {
_state = Flip_Recover;
_state = FlipState::Recover;
}
break;
case Flip_Pitch_A:
case FlipState::Pitch_A:
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, FLIP_ROTATION_RATE * pitch_dir, 0.0);
// decrease throttle
@ -157,11 +157,11 @@ void Copter::ModeFlip::run()
// check roll for inversion
if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) {
_state = Flip_Pitch_B;
_state = FlipState::Pitch_B;
}
break;
case Flip_Pitch_B:
case FlipState::Pitch_B:
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control->input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * pitch_dir, 0.0);
// decrease throttle
@ -169,11 +169,11 @@ void Copter::ModeFlip::run()
// check roll for inversion
if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) {
_state = Flip_Recover;
_state = FlipState::Recover;
}
break;
case Flip_Recover: {
case FlipState::Recover: {
// use originally captured earth-frame angle targets to recover
attitude_control->input_euler_angle_roll_pitch_yaw(orig_attitude.x, orig_attitude.y, orig_attitude.z, false);
@ -201,7 +201,7 @@ void Copter::ModeFlip::run()
}
break;
}
case Flip_Abandon:
case FlipState::Abandon:
// restore original flight mode
if (!copter.set_mode(orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
// this should never happen but just in case