aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: FlightMode - simplify flight mode initialization

Browse Source
This commit is contained in:
Peter Barker 2016-03-23 14:27:05 +11:00 committed by Randy Mackay
parent 0ca5605b8d
commit 83d0a71e10
2 changed files with 122 additions and 182 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -1010,6 +1010,8 @@ private:
Copter::FlightMode_SMARTRTL flightmode_smartrtl{*this}; Copter::FlightMode_SMARTRTL flightmode_smartrtl{*this};
Copter::FlightMode *flightmode_for_mode(const uint8_t mode);
public: public:
void mavlink_delay_cb(); void mavlink_delay_cb();
void failsafe_check(); void failsafe_check();

302
ArduCopter/flight_mode.cpp
View File

@ -5,197 +5,144 @@
* flight modes is in control_acro.cpp, control_stabilize.cpp, etc * flight modes is in control_acro.cpp, control_stabilize.cpp, etc
*/ */
// return the static controller object corresponding to supplied mode
Copter::FlightMode *Copter::flightmode_for_mode(const uint8_t mode)
{
Copter::FlightMode *ret = nullptr;
switch (mode) {
case ACRO:
ret = &flightmode_acro;
break;
case STABILIZE:
ret = &flightmode_stabilize;
break;
case ALT_HOLD:
ret = &flightmode_althold;
break;
case AUTO:
ret = &flightmode_auto;
break;
case CIRCLE:
ret = &flightmode_circle;
break;
case LOITER:
ret = &flightmode_loiter;
break;
case GUIDED:
ret = &flightmode_guided;
break;
case LAND:
ret = &flightmode_land;
break;
case RTL:
ret = &flightmode_rtl;
break;
case DRIFT:
ret = &flightmode_drift;
break;
case SPORT:
ret = &flightmode_sport;
break;
case FLIP:
ret = &flightmode_flip;
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
ret = &flightmode_autotune;
break;
#endif
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
ret = &flightmode_poshold;
break;
#endif
case BRAKE:
ret = &flightmode_brake;
break;
case THROW:
ret = &flightmode_throw;
break;
case AVOID_ADSB:
ret = &flightmode_avoid_adsb;
break;
case GUIDED_NOGPS:
ret = &flightmode_guided_nogps;
break;
case SMART_RTL:
ret = &flightmode_smartrtl;
break;
default:
break;
}
return ret;
}
// set_mode - change flight mode and perform any necessary initialisation // set_mode - change flight mode and perform any necessary initialisation
// optional force parameter used to force the flight mode change (used only first time mode is set) // optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was successfully set // returns true if mode was successfully set
// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately // ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
{ {
// boolean to record if flight mode could be set
bool success = false;
bool ignore_checks = !motors->armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
// return immediately if we are already in the desired mode // return immediately if we are already in the desired mode
if (mode == control_mode) { if (mode == control_mode) {
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode_reason = reason; control_mode_reason = reason;
return true; return true;
} }
Copter::FlightMode *new_flightmode = flightmode_for_mode(mode);
if (new_flightmode == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING,"No such mode");
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
return false;
}
bool ignore_checks = !motors->armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
if (! new_flightmode->init(ignore_checks)) {
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
return false;
}
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter a non-manual throttle mode if the // do not allow helis to enter a non-manual throttle mode if the
// rotor runup is not complete // rotor runup is not complete
if (!ignore_checks && !mode_has_manual_throttle(mode) && !motors->rotor_runup_complete()){ if (!ignore_checks && !mode_has_manual_throttle(mode) && !motors->rotor_runup_complete()){
goto failed; return false;
} }
#endif #endif
// for transition, we assume no flightmode object will be used in // perform any cleanup required by previous flight mode
// the new mode, and if the transition fails we reset the exit_mode(control_mode, mode);
// flightmode to the previous value
Copter::FlightMode* old_flightmode = flightmode;
flightmode = nullptr;
switch (mode) {
case ACRO:
success = flightmode_acro.init(ignore_checks);
if (success) {
flightmode = &flightmode_acro;
}
break;
case STABILIZE:
success = flightmode_stabilize.init(ignore_checks);
if (success) {
flightmode = &flightmode_stabilize;
}
break;
case ALT_HOLD:
success = flightmode_althold.init(ignore_checks);
if (success) {
flightmode = &flightmode_althold;
}
break;
case AUTO:
success = flightmode_auto.init(ignore_checks);
if (success) {
flightmode = &flightmode_auto;
}
break;
case CIRCLE:
success = flightmode_circle.init(ignore_checks);
if (success) {
flightmode = &flightmode_circle;
}
break;
case LOITER:
success = flightmode_loiter.init(ignore_checks);
if (success) {
flightmode = &flightmode_loiter;
}
break;
case GUIDED:
success = flightmode_guided.init(ignore_checks);
if (success) {
flightmode = &flightmode_guided;
}
break;
case LAND:
success = flightmode_land.init(ignore_checks);
if (success) {
flightmode = &flightmode_land;
}
break;
case RTL:
success = flightmode_rtl.init(ignore_checks);
if (success) {
flightmode = &flightmode_rtl;
}
break;
case DRIFT:
success = flightmode_drift.init(ignore_checks);
if (success) {
flightmode = &flightmode_drift;
}
break;
case SPORT:
success = flightmode_sport.init(ignore_checks);
if (success) {
flightmode = &flightmode_sport;
}
break;
case FLIP:
success = flightmode_flip.init(ignore_checks);
if (success) {
flightmode = &flightmode_flip;
}
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
success = flightmode_autotune.init(ignore_checks);
if (success) {
flightmode = &flightmode_autotune;
}
break;
#endif
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
success = flightmode_poshold.init(ignore_checks);
if (success) {
flightmode = &flightmode_poshold;
}
break;
#endif
case BRAKE:
success = flightmode_brake.init(ignore_checks);
if (success) {
flightmode = &flightmode_brake;
}
break;
case THROW:
success = flightmode_throw.init(ignore_checks);
if (success) {
flightmode = &flightmode_throw;
}
break;
case AVOID_ADSB:
success = flightmode_avoid_adsb.init(ignore_checks);
if (success) {
flightmode = &flightmode_avoid_adsb;
}
break;
case GUIDED_NOGPS:
success = flightmode_guided_nogps.init(ignore_checks);
if (success) {
flightmode = &flightmode_guided_nogps;
}
break;
case SMART_RTL:
success = flightmode_guided_nogps.init(ignore_checks);
if (success) {
flightmode = &flightmode_smartrtl;
}
break;
default:
success = false;
break;
}
#if FRAME_CONFIG == HELI_FRAME
failed:
#endif
// update flight mode // update flight mode
if (success) { flightmode = new_flightmode;
// perform any cleanup required by previous flight mode control_mode = mode;
exit_mode(control_mode, mode); control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode);
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode = mode;
control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED)); adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED));
@ -203,7 +150,7 @@ failed:
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
// this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
// but it should be harmless to disable the fence temporarily in these situations as well // but it should be harmless to disable the fence temporarily in these situations as well
fence.manual_recovery_start(); fence.manual_recovery_start();
#endif #endif
#if FRSKY_TELEM_ENABLED == ENABLED #if FRSKY_TELEM_ENABLED == ENABLED
@ -212,21 +159,12 @@ failed:
#if CAMERA == ENABLED #if CAMERA == ENABLED
camera.set_is_auto_mode(control_mode == AUTO); camera.set_is_auto_mode(control_mode == AUTO);
#endif #endif
} else {
flightmode = old_flightmode;
// Log error that we failed to enter desired flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
}
// update notify object // update notify object
if (success) { notify_flight_mode();
notify_flight_mode();
}
// return success or failure // return success
return success; return true;
} }
// update_flight_mode - calls the appropriate attitude controllers based on flight mode // update_flight_mode - calls the appropriate attitude controllers based on flight mode