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

Copter: factor out a mode-change-failed method

This commit is contained in:
Peter Barker 2020-07-06 14:32:32 +10:00 committed by Peter Barker
parent f0d6b79e8a
commit 5edfed0083
2 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -803,6 +803,8 @@ private:
// mode.cpp
bool set_mode(Mode::Number mode, ModeReason reason);
bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
// called when an attempt to change into a mode is unsuccessful:
void mode_change_failed(const Mode *mode, const char *reason);
uint8_t get_mode() const override { return (uint8_t)flightmode->mode_number(); }
void update_flight_mode();
void notify_flight_mode();

22
ArduCopter/mode.cpp
View File

@ -177,6 +177,13 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
}
// called when an attempt to change into a mode is unsuccessful:
void Copter::mode_change_failed(const Mode *mode, const char *reason)
{
gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to %s failed: %s", mode->name(), reason);
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode->mode_number()));
}
// 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)
// returns true if mode was successfully set
@ -213,8 +220,7 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
#endif
if (!in_autorotation_check) {
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name());
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
mode_change_failed(new_flightmode, "runup not complete");
return false;
}
}
@ -236,8 +242,7 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
user_throttle &&
!copter.flightmode->has_manual_throttle() &&
new_flightmode->get_pilot_desired_throttle() > copter.get_non_takeoff_throttle()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: throttle too high");
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
mode_change_failed(new_flightmode, "throttle too high");
return false;
}
#endif
@ -245,8 +250,7 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
if (!ignore_checks &&
new_flightmode->requires_GPS() &&
!copter.position_ok()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name());
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
mode_change_failed(new_flightmode, "requires position");
return false;
}
@ -256,14 +260,12 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
!copter.ekf_alt_ok() &&
flightmode->has_manual_throttle() &&
!new_flightmode->has_manual_throttle()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name());
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
mode_change_failed(new_flightmode, "need alt estimate");
return false;
}
if (!new_flightmode->init(ignore_checks)) {
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name());
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
mode_change_failed(new_flightmode, "initialisation failed");
return false;
}