You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
57 lines
1.8 KiB
57 lines
1.8 KiB
/* |
|
* AP_ArmingMechanism.cpp |
|
* |
|
*/ |
|
|
|
|
|
#include "AP_ArmingMechanism.h" |
|
#include "AP_RcChannel.h" |
|
#include "../FastSerial/FastSerial.h" |
|
#include "AP_HardwareAbstractionLayer.h" |
|
#include "AP_CommLink.h" |
|
|
|
namespace apo { |
|
|
|
void AP_ArmingMechanism::update(const float dt) { |
|
//_hal->debug->printf_P(PSTR("ch1: %f\tch2: %f\n"),_hal->rc[_ch1]->getRadioPosition(), _hal->rc[_ch2]->getRadioPosition()); |
|
// arming |
|
if ( (_hal->getState() != MAV_STATE_ACTIVE) && |
|
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) && |
|
(_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) { |
|
|
|
// always start clock at 0 |
|
if (_armingClock<0) _armingClock = 0; |
|
|
|
if (_armingClock++ >= 100) { |
|
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); |
|
_hal->setState(MAV_STATE_ACTIVE); |
|
} else { |
|
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming")); |
|
} |
|
} |
|
// disarming |
|
else if ( (_hal->getState() == MAV_STATE_ACTIVE) && |
|
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) && |
|
(_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) { |
|
|
|
// always start clock at 0 |
|
if (_armingClock>0) _armingClock = 0; |
|
|
|
if (_armingClock-- <= -100) { |
|
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); |
|
_hal->setState(MAV_STATE_STANDBY); |
|
} else { |
|
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming")); |
|
} |
|
} |
|
// reset arming clock and report status |
|
else if (_armingClock != 0) { |
|
_armingClock = 0; |
|
if (_hal->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); |
|
else if (_hal->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); |
|
} |
|
} |
|
|
|
} // apo |
|
|
|
// vim:ts=4:sw=4:expandtab
|
|
|