diff --git a/libraries/APO/AP_ArmingMechanism.cpp b/libraries/APO/AP_ArmingMechanism.cpp index 8d5a67e3c9..e27d19a246 100644 --- a/libraries/APO/AP_ArmingMechanism.cpp +++ b/libraries/APO/AP_ArmingMechanism.cpp @@ -14,42 +14,44 @@ namespace apo { void AP_ArmingMechanism::update(const float dt) { - // arming - if ( (_hal->getState() != MAV_STATE_ACTIVE) & - (_hal->rc[_ch1]->getRadioPosition() < _ch1Min) && - (_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) { + // arming + if ( (_hal->getState() != MAV_STATE_ACTIVE) & + (_hal->rc[_ch1]->getRadioPosition() < _ch1Min) && + (_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) { - // always start clock at 0 - if (_armingClock<0) _armingClock = 0; + // 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) & - (_hal->rc[_ch1]->getRadioPosition() < _ch1Min) && - (_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) { + 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) & + (_hal->rc[_ch1]->getRadioPosition() < _ch1Min) && + (_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) { - // always start clock at 0 - if (_armingClock>0) _armingClock = 0; + // 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")); - } + 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 diff --git a/libraries/APO/AP_ArmingMechanism.h b/libraries/APO/AP_ArmingMechanism.h index 6054df3ae9..1f68afd1f5 100644 --- a/libraries/APO/AP_ArmingMechanism.h +++ b/libraries/APO/AP_ArmingMechanism.h @@ -16,50 +16,52 @@ class AP_HardwareAbstractionLayer; class AP_ArmingMechanism { public: - /** - * Constructor - * - * @param ch1: typically throttle channel - * @param ch2: typically yaw channel - * @param ch1Min: disarms/arms belows this - * @param ch2Min: disarms below this - * @param ch2Max: arms above this - */ - AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal, - uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min, - float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2), - _ch1Min(ch1Min), _ch2Min(ch2Min) { - } + /** + * Constructor + * + * @param ch1: typically throttle channel + * @param ch2: typically yaw channel + * @param ch1Min: disarms/arms belows this + * @param ch2Min: disarms below this + * @param ch2Max: arms above this + */ + AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal, + uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min, + float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2), + _ch1Min(ch1Min), _ch2Min(ch2Min) { + } - /** - * update - * - * arming: - * - * to arm: put stick to bottom right for 100 controller cycles - * (max yaw, min throttle) - * - * didn't use clock here in case of millis() roll over - * for long runs - * - * disarming: - * - * to disarm: put stick to bottom left for 100 controller cycles - * (min yaw, min throttle) - */ - void update(const float dt); + /** + * update + * + * arming: + * + * to arm: put stick to bottom right for 100 controller cycles + * (max yaw, min throttle) + * + * didn't use clock here in case of millis() roll over + * for long runs + * + * disarming: + * + * to disarm: put stick to bottom left for 100 controller cycles + * (min yaw, min throttle) + */ + void update(const float dt); private: - AP_HardwareAbstractionLayer * _hal; - uint8_t _armingClock; - uint8_t _ch1; /// typically throttle channel - uint8_t _ch2; /// typically yaw channel - float _ch1Min; /// arms/disarms below this on ch1 - float _ch2Min; /// disarms below this on ch2 - float _ch2Max; /// arms above this on ch2 + AP_HardwareAbstractionLayer * _hal; + uint8_t _armingClock; + uint8_t _ch1; /// typically throttle channel + uint8_t _ch2; /// typically yaw channel + float _ch1Min; /// arms/disarms below this on ch1 + float _ch2Min; /// disarms below this on ch2 + float _ch2Max; /// arms above this on ch2 }; } // namespace apo #endif /* AP_ARMINGMECHANISM */ + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 0944620247..15b1e1e40b 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -146,3 +146,4 @@ private: #endif // AP_Guide_H // vim:ts=4:sw=4:expandtab +