diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 847f5617fd..466e9add86 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -10,6 +10,7 @@ #include "../APO/AP_Controller.h" #include "../APO/AP_BatteryMonitor.h" +#include "../APO/AP_ArmingMechanism.h" namespace apo { @@ -78,7 +79,7 @@ public: PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT), pidPD(new AP_Var_group(k_pidPD, PSTR("DOWN_")), 1, PID_POS_Z_P, PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_DFCUT), - _armingClock(0), _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0), + _armingMechanism(hal,CH_THRUST,CH_YAW,0.1,-0.9,0.9), _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0), _cmdRoll(0), _cmdPitch(0), _cmdYawRate(0), _mode(MAV_MODE_LOCKED) { /* * allocate radio channels @@ -117,51 +118,7 @@ public: virtual void update(const float & dt) { //_hal->debug->printf_P(PSTR("thr: %f, yaw: %f\n"),_hal->rc[CH_THRUST]->getRadioPosition(),_hal->rc[CH_YAW]->getRadioPosition()); - // 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 - if ( (_hal->getState() != MAV_STATE_ACTIVE) & - (_hal->rc[CH_THRUST]->getRadioPosition() < 0.1) && - (_hal->rc[CH_YAW]->getRadioPosition() < -0.9) ) { - - // 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 - // - // to disarm: put stick to bottom left for 100 controller cycles - // (min yaw, min throttle) - else if ( (_hal->getState() == MAV_STATE_ACTIVE) & - (_hal->rc[CH_THRUST]->getRadioPosition() < 0.1) && - (_hal->rc[CH_YAW]->getRadioPosition() > 0.9) ) { - - // 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")); - } + _armingMechanism.update(dt); // determine flight mode // @@ -232,7 +189,7 @@ private: BlockPIDDfb pidRoll, pidPitch, pidYaw; BlockPID pidYawRate; BlockPIDDfb pidPN, pidPE, pidPD; - int32_t _armingClock; + AP_ArmingMechanism _armingMechanism; float _thrustMix, _pitchMix, _rollMix, _yawMix; float _cmdRoll, _cmdPitch, _cmdYawRate; diff --git a/libraries/APO/AP_ArmingMechanism.cpp b/libraries/APO/AP_ArmingMechanism.cpp new file mode 100644 index 0000000000..8d5a67e3c9 --- /dev/null +++ b/libraries/APO/AP_ArmingMechanism.cpp @@ -0,0 +1,55 @@ +/* + * 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) { + + // 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; + + 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; + + 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 diff --git a/libraries/APO/AP_ArmingMechanism.h b/libraries/APO/AP_ArmingMechanism.h new file mode 100644 index 0000000000..6054df3ae9 --- /dev/null +++ b/libraries/APO/AP_ArmingMechanism.h @@ -0,0 +1,65 @@ +/* + * AP_ArmingMechanism.h + * + */ + +#ifndef AP_ARMINGMECHANISM_H_ +#define AP_ARMINGMECHANISM_H_ + +#include +#include + +namespace apo { + +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) { + } + + /** + * 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 +}; + +} // namespace apo + +#endif /* AP_ARMINGMECHANISM */