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

38
libraries/APO/AP_Controller.cpp
View File

@ -43,6 +43,24 @@ 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 modes
//
// if in locked mode
if (getMode() == MAV_MODE_LOCKED) {
// if state is not stanby then set it to standby and alert gcs
if (getState()!=MAV_STATE_STANDBY) {
setState(MAV_STATE_STANDBY);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
}
}
// 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 // handle emergencies
// //
@ -57,24 +75,6 @@ void AP_Controller::update(const float dt) {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery")); _hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
} }
// handle modes
//
// if in locked mode
if (getMode() == MAV_MODE_LOCKED) {
// if state is not stanby then set it to standby and alert gcs
if (getState()!=MAV_STATE_STANDBY) {
setState(MAV_STATE_STANDBY);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
}
}
// 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"));
}
// 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);