Added arming mechanism.

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;

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

libraries/APO/AP_ArmingMechanism.h

@@ -0,0 +1,65 @@
+/*
+ * AP_ArmingMechanism.h
+ *
+ */
+
+#ifndef AP_ARMINGMECHANISM_H_
+#define AP_ARMINGMECHANISM_H_
+
+#include <inttypes.h>
+#include <wiring.h>
+
+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 */