Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: gracefully-ish fail set_mode(qmode)

Browse Source
This commit is contained in:
Tom Pittenger 2019-04-01 10:54:34 -07:00 committed by Andrew Tridgell
parent b8a44e6e52
commit fbe483ccac
2 changed files with 30 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduPlane/mode.h
View File

@ -62,6 +62,9 @@ public:
// convert user input to targets, implement high level control for this mode
virtual void update() = 0;
// true for all q modes
virtual bool is_vtol_mode() const { return false; }
protected:
// subclasses override this to perform checks before entering the mode
@ -326,6 +329,8 @@ public:
const char *name() const override { return "QSTABILIZE"; }
const char *name4() const override { return "QSTB"; }
bool is_vtol_mode() const override { return true; }
// methods that affect movement of the vehicle in this mode
void update() override;
@ -344,6 +349,8 @@ public:
const char *name() const override { return "QHOVER"; }
const char *name4() const override { return "QHOV"; }
bool is_vtol_mode() const override { return true; }
// methods that affect movement of the vehicle in this mode
void update() override;
@ -360,6 +367,8 @@ public:
const char *name() const override { return "QLOITER"; }
const char *name4() const override { return "QLOT"; }
bool is_vtol_mode() const override { return true; }
// methods that affect movement of the vehicle in this mode
void update() override;
@ -376,6 +385,8 @@ public:
const char *name() const override { return "QLAND"; }
const char *name4() const override { return "QLND"; }
bool is_vtol_mode() const override { return true; }
// methods that affect movement of the vehicle in this mode
void update() override;
@ -392,6 +403,8 @@ public:
const char *name() const override { return "QRTL"; }
const char *name4() const override { return "QRTL"; }
bool is_vtol_mode() const override { return true; }
// methods that affect movement of the vehicle in this mode
void update() override;
@ -408,6 +421,8 @@ public:
const char *name() const override { return "QACO"; }
const char *name4() const override { return "QACRO"; }
bool is_vtol_mode() const override { return true; }
// methods that affect movement of the vehicle in this mode
void update() override;
@ -424,6 +439,8 @@ public:
const char *name() const override { return "QAUTOTUNE"; }
const char *name4() const override { return "QATN"; }
bool is_vtol_mode() const override { return true; }
// methods that affect movement of the vehicle in this mode
void update() override;

14
ArduPlane/system.cpp
View File

@ -245,13 +245,15 @@ bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason)
#if !QAUTOTUNE_ENABLED
if (&new_mode == plane.mode_qautotune) {
gcs().send_text(MAV_SEVERITY_INFO,"QAUTOTUNE disabled");
new_mode = plane.mode_qhover;
set_mode(plane.mode_qhover);
return;
}
#endif
// backup current control_mode and previous_mode
Mode &old_previous_mode = *previous_mode;
Mode &old_mode = *control_mode;
const mode_reason_t previous_mode_reason_backup = previous_mode_reason;
// update control_mode assuming success
// TODO: move these to be after enter() once start_command_callback() no longer checks control_mode
@ -268,6 +270,16 @@ bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason)
// we failed entering new mode, roll back to old
previous_mode = &old_previous_mode;
control_mode = &old_mode;
control_mode_reason = previous_mode_reason;
previous_mode_reason = previous_mode_reason_backup;
// currently, only Q modes can fail enter(). This will likely change in the future and all modes
// should be changed to check dependencies and fail early before depending on changes in Mode::set_mode()
if (control_mode->is_vtol_mode()) {
// ignore result because if we fail we risk looping at the qautotune check above
control_mode->enter();
}
return false;
}