Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: FlightMode - convert FLIP flight mode

Browse Source
This commit is contained in:
Peter Barker 2016-03-22 21:41:38 +11:00 committed by Randy Mackay
parent 5e3e831152
commit 018c70d224
4 changed files with 50 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/Copter.h
View File

@ -419,9 +419,6 @@ private:
int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.)
uint32_t nav_delay_time_start;
// Flip
Vector3f flip_orig_attitude; // original vehicle attitude before flip
// throw mode state
struct {
ThrowModeStage stage;
@ -817,8 +814,6 @@ private:
bool brake_init(bool ignore_checks);
void brake_run();
void brake_timeout_to_loiter_ms(uint32_t timeout_ms);
bool flip_init(bool ignore_checks);
void flip_run();
bool guided_nogps_init(bool ignore_checks);
void guided_nogps_run();
void land_run_vertical_control(bool pause_descent = false);
@ -1064,6 +1059,8 @@ private:
Copter::FlightMode_DRIFT flightmode_drift{*this};
Copter::FlightMode_FLIP flightmode_flip{*this};
Copter::FlightMode_GUIDED flightmode_guided{*this};
Copter::FlightMode_LAND flightmode_land{*this};

30
ArduCopter/FlightMode.h
View File

@ -633,3 +633,33 @@ protected:
private:
};
class FlightMode_FLIP : public FlightMode {
public:
FlightMode_FLIP(Copter &copter) :
Copter::FlightMode(copter)
{ }
virtual bool init(bool ignore_checks) override;
virtual void run() override; // should be called at 100hz or more
virtual bool requires_GPS() const override { return false; }
virtual bool has_manual_throttle() const override { return false; }
virtual bool allows_arming(bool from_gcs) const override { return false; };
virtual bool is_autopilot() const override { return false; }
protected:
const char *name() const override { return "FLIP"; }
const char *name4() const override { return "FLIP"; }
private:
// Flip
Vector3f flip_orig_attitude; // original vehicle attitude before flip
};

26
ArduCopter/control_flip.cpp
View File

@ -30,6 +30,7 @@
#define FLIP_PITCH_BACK 1 // used to set flip_dir
#define FLIP_PITCH_FORWARD -1 // used to set flip_dir
// FIXME? these should be instance variables?
FlipState flip_state; // current state of flip
control_mode_t flip_orig_control_mode; // flight mode when flip was initated
uint32_t flip_start_time; // time since flip began
@ -37,15 +38,15 @@ int8_t flip_roll_dir; // roll direction (-1 = roll left, 1 = roll
int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back)
// flip_init - initialise flip controller
bool Copter::flip_init(bool ignore_checks)
bool Copter::FlightMode_FLIP::init(bool ignore_checks)
{
// only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) {
if (_copter.control_mode != ACRO && _copter.control_mode != STABILIZE && _copter.control_mode != ALT_HOLD) {
return false;
}
// if in acro or stabilize ensure throttle is above zero
if (ap.throttle_zero && (control_mode == ACRO || control_mode == STABILIZE)) {
if (ap.throttle_zero && (_copter.control_mode == ACRO || _copter.control_mode == STABILIZE)) {
return false;
}
@ -60,7 +61,7 @@ bool Copter::flip_init(bool ignore_checks)
}
// capture original flight mode so that we can return to it after completion
flip_orig_control_mode = control_mode;
flip_orig_control_mode = _copter.control_mode;
// initialise state
flip_state = Flip_Start;
@ -83,8 +84,9 @@ bool Copter::flip_init(bool ignore_checks)
Log_Write_Event(DATA_FLIP_START);
// capture current attitude which will be used during the Flip_Recovery stage
flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -aparm.angle_max, aparm.angle_max);
flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -aparm.angle_max, aparm.angle_max);
float angle_max = _copter.aparm.angle_max;
flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -angle_max, angle_max);
flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -angle_max, angle_max);
flip_orig_attitude.z = ahrs.yaw_sensor;
return true;
@ -92,7 +94,7 @@ bool Copter::flip_init(bool ignore_checks)
// flip_run - runs the flip controller
// should be called at 100hz or more
void Copter::flip_run()
void Copter::FlightMode_FLIP::run()
{
float throttle_out;
float recovery_angle;
@ -191,9 +193,9 @@ void Copter::flip_run()
// check for successful recovery
if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
// restore original flight mode
if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
if (!_copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
// this should never happen but just in case
set_mode(STABILIZE, MODE_REASON_UNKNOWN);
_copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
}
// log successful completion
Log_Write_Event(DATA_FLIP_END);
@ -202,12 +204,12 @@ void Copter::flip_run()
case Flip_Abandon:
// restore original flight mode
if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
if (!_copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
// this should never happen but just in case
set_mode(STABILIZE, MODE_REASON_UNKNOWN);
_copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
}
// log abandoning flip
Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED);
_copter.Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED);
break;
}

12
ArduCopter/flight_mode.cpp
View File

@ -117,7 +117,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
break;
case FLIP:
success = flip_init(ignore_checks);
success = flightmode_flip.init(ignore_checks);
if (success) {
flightmode = &flightmode_flip;
}
break;
#if AUTOTUNE_ENABLED == ENABLED
@ -218,10 +221,6 @@ void Copter::update_flight_mode()
switch (control_mode) {
case FLIP:
flip_run();
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
autotune_run();
@ -382,9 +381,6 @@ void Copter::notify_flight_mode()
// set flight mode string
switch (control_mode) {
case FLIP:
notify.set_flight_mode_str("FLIP");
break;
case AUTOTUNE:
notify.set_flight_mode_str("ATUN");
break;