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

Fixed mode switching.

Browse Source
This commit is contained in:
James Goppert 2011-11-27 23:55:28 -05:00
parent 8dcca613b7
commit 21a27ee083
1 changed files with 16 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/APO/AP_Controller.cpp
View File

@ -43,20 +43,6 @@ void AP_Controller::setAllRadioChannelsManually() {
void AP_Controller::update(const float dt) { void AP_Controller::update(const float dt) {
if (_armingMechanism) _armingMechanism->update(dt); if (_armingMechanism) _armingMechanism->update(dt);
// handle emergencies
//
// check for heartbeat from gcs, if not found go to failsafe
if (_hal->heartBeatLost()) {
setMode(MAV_MODE_FAILSAFE);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
// if battery less than 5%, go to failsafe
} else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) {
setMode(MAV_MODE_FAILSAFE);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
}
// handle modes // handle modes
// //
// if in locked mode // if in locked mode
@ -69,12 +55,26 @@ void AP_Controller::update(const float dt) {
} }
// if not locked // if not locked
else { else {
// if state is not active, set it to active and alert gcs // if state is not active, set it to active and alert gcs
if (getState()!=MAV_STATE_ACTIVE) { if (getState()!=MAV_STATE_ACTIVE) {
setState(MAV_STATE_ACTIVE); setState(MAV_STATE_ACTIVE);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
} }
// handle emergencies
//
// check for heartbeat from gcs, if not found go to failsafe
if (_hal->heartBeatLost()) {
setMode(MAV_MODE_FAILSAFE);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
// if battery less than 5%, go to failsafe
} else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) {
setMode(MAV_MODE_FAILSAFE);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
}
// if in auto mode and manual switch set, change to manual // if in auto mode and manual switch set, change to manual
if (_hal->rc[_chMode]->getRadioPosition() > 0) setMode(MAV_MODE_MANUAL); if (_hal->rc[_chMode]->getRadioPosition() > 0) setMode(MAV_MODE_MANUAL);
else setMode(MAV_MODE_AUTO); else setMode(MAV_MODE_AUTO);
@ -84,6 +84,8 @@ void AP_Controller::update(const float dt) {
manualLoop(dt); manualLoop(dt);
} else if (getMode()==MAV_MODE_AUTO) { } else if (getMode()==MAV_MODE_AUTO) {
autoLoop(dt); autoLoop(dt);
} else if (getMode()==MAV_MODE_FAILSAFE) {
handleFailsafe();
} else { } else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode")); _hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
setMode(MAV_MODE_FAILSAFE); setMode(MAV_MODE_FAILSAFE);