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

Fixed mode switching.

This commit is contained in:
James Goppert 2011-11-27 23:55:28 -05:00
parent 64de8371a2
commit 999a5892f0
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) {
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
//
// if in locked mode
@ -69,12 +55,26 @@ void AP_Controller::update(const float dt) {
}
// if not locked
else {
// if state is not active, set it to active and alert gcs
if (getState()!=MAV_STATE_ACTIVE) {
setState(MAV_STATE_ACTIVE);
_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 (_hal->rc[_chMode]->getRadioPosition() > 0) setMode(MAV_MODE_MANUAL);
else setMode(MAV_MODE_AUTO);
@ -84,6 +84,8 @@ void AP_Controller::update(const float dt) {
manualLoop(dt);
} else if (getMode()==MAV_MODE_AUTO) {
autoLoop(dt);
} else if (getMode()==MAV_MODE_FAILSAFE) {
handleFailsafe();
} else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
setMode(MAV_MODE_FAILSAFE);