ardupilot/libraries/APO/AP_ArmingMechanism.cpp

56 lines
1.7 KiB
C++
Raw Normal View History

2011-10-19 01:21:19 -03:00
/*
* AP_ArmingMechanism.cpp
*
*/
#include "AP_ArmingMechanism.h"
#include "AP_Controller.h"
2011-10-19 01:21:19 -03:00
#include "AP_RcChannel.h"
#include "../FastSerial/FastSerial.h"
2011-12-07 17:31:56 -04:00
#include "AP_Board.h"
2011-10-19 01:21:19 -03:00
#include "AP_CommLink.h"
namespace apo {
void AP_ArmingMechanism::update(const float dt) {
2011-12-07 17:31:56 -04:00
//_board->debug->printf_P(PSTR("ch1: %f\tch2: %f\n"),_board->rc[_ch1]->getRadioPosition(), _board->rc[_ch2]->getRadioPosition());
2011-10-19 01:25:00 -03:00
// arming
if ( (_controller->getState() != MAV_STATE_ACTIVE) &&
2011-12-07 17:31:56 -04:00
(fabs(_board->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_board->rc[_ch2]->getRadioPosition() < _ch2Min) ) {
2011-10-19 01:25:00 -03:00
// always start clock at 0
if (_armingClock<0) _armingClock = 0;
if (_armingClock++ >= 100) {
_controller->setMode(MAV_MODE_READY);
2011-10-19 01:25:00 -03:00
} else {
2011-12-07 17:31:56 -04:00
_board->gcs->sendText(SEVERITY_HIGH, PSTR("arming"));
2011-10-19 01:25:00 -03:00
}
}
// disarming
else if ( (_controller->getState() == MAV_STATE_ACTIVE) &&
2011-12-07 17:31:56 -04:00
(fabs(_board->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_board->rc[_ch2]->getRadioPosition() > _ch2Max) ) {
2011-10-19 01:25:00 -03:00
// always start clock at 0
if (_armingClock>0) _armingClock = 0;
if (_armingClock-- <= -100) {
_controller->setMode(MAV_MODE_LOCKED);
2011-10-19 01:25:00 -03:00
} else {
2011-12-07 17:31:56 -04:00
_board->gcs->sendText(SEVERITY_HIGH, PSTR("disarming"));
2011-10-19 01:25:00 -03:00
}
}
// reset arming clock and report status
else if (_armingClock != 0) {
_armingClock = 0;
2011-12-07 17:31:56 -04:00
if (_controller->getState()==MAV_STATE_ACTIVE) _board->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
else if (_controller->getState()!=MAV_STATE_ACTIVE) _board->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
2011-10-19 01:25:00 -03:00
}
2011-10-19 01:21:19 -03:00
}
} // apo
2011-10-19 01:25:00 -03:00
// vim:ts=4:sw=4:expandtab