Further abstracted APO Controllers

Currently APO quad/plane/tank need to be updated
for new controller changes.

apo/PlaneEasystar.h

@@ -10,8 +10,8 @@
 
 // vehicle options
 static const apo::vehicle_t vehicle = apo::VEHICLE_PLANE;
-static const apo::halMode_t halMode = apo::MODE_LIVE;
-static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560;
+static const apo::halMode_t halMode = apo::MODE_HIL_CNTL;
+static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
 static const uint8_t heartBeatTimeout = 3;
 
 // algorithm selection

apo/QuadArducopter.h

@@ -10,7 +10,7 @@
 
 // vehicle options
 static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD;
-static const apo::halMode_t halMode = apo::MODE_LIVE;
+static const apo::halMode_t halMode = apo::MODE_HIL_CNTL;
 static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
 static const uint8_t heartBeatTimeout = 3;
 

apo/apo.pde

@@ -17,8 +17,8 @@
 #include <WProgram.h>
 
 // Vehicle Configuration
-#include "QuadArducopter.h"
-//#include "PlaneEasystar.h"
+//#include "QuadArducopter.h"
+#include "PlaneEasystar.h"
 
 // ArduPilotOne Default Setup
 #include "APO_DefaultSetup.h"

libraries/APO/APO.h

@@ -16,6 +16,6 @@
 #include "AP_Navigator.h"
 #include "AP_RcChannel.h"
 #include "AP_BatteryMonitor.h"
-//#include "AP_Var_keys.h"
+#include "AP_ArmingMechanism.h"
 
 #endif /* APO_H_ */

libraries/APO/APO_DefaultSetup.h

@@ -138,8 +138,8 @@ void setup() {
 	 * Select guidance, navigation, control algorithms
 	 */
 	navigator = new NAVIGATOR_CLASS(hal);
-	guide = new GUIDE_CLASS(navigator, hal);
-	controller = new CONTROLLER_CLASS(navigator, guide, hal);
+	guide = new GUIDE_CLASS(navigator, hal, velCmd, xt, xtLim);
+	controller = new CONTROLLER_CLASS(navigator,guide,hal);
 
 	/*
 	 * CommLinks
@@ -161,6 +161,8 @@ void setup() {
 	if (hal->getMode() == MODE_HIL_CNTL) {
 		Serial.println("HIL line setting up");
 		Serial1.begin(hilBaud, 128, 128);
+		delay(1000);
+		Serial1.println("starting hil");
 		hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
 	}
 

libraries/APO/AP_ArmingMechanism.cpp

@@ -16,7 +16,7 @@ void AP_ArmingMechanism::update(const float dt) {
 
     // arming
     if ( (_hal->getState() != MAV_STATE_ACTIVE) &&
-     (_hal->rc[_ch1]->getRadioPosition() < _ch1Min) &&
+     (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
      (_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) {
 
         // always start clock at 0
@@ -31,7 +31,7 @@ void AP_ArmingMechanism::update(const float dt) {
     }
     // disarming
     else if ( (_hal->getState() == MAV_STATE_ACTIVE) &&
-         (_hal->rc[_ch1]->getRadioPosition() < _ch1Min) &&
+         (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
          (_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) {
 
         // always start clock at 0

libraries/APO/AP_Controller.cpp

@@ -6,15 +6,25 @@
  */
 
 #include "AP_Controller.h"
+#include "../FastSerial/FastSerial.h"
+#include "AP_ArmingMechanism.h"
+#include "AP_BatteryMonitor.h"
 #include "AP_HardwareAbstractionLayer.h"
 #include "../AP_Common/include/menu.h"
 #include "AP_RcChannel.h"
+#include "../GCS_MAVLink/include/mavlink_types.h"
+#include "constants.h"
+#include "AP_CommLink.h"
 
 namespace apo {
 
 AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide,
-		AP_HardwareAbstractionLayer * hal) :
-	_nav(nav), _guide(guide), _hal(hal) {
+		AP_HardwareAbstractionLayer * hal, AP_ArmingMechanism * armingMechanism,
+		const uint8_t chMode, const uint16_t key, const prog_char_t * name) :
+		_nav(nav), _guide(guide), _hal(hal), _armingMechanism(armingMechanism),
+		_group(key, name ? : PSTR("CNTRL_")),
+		_chMode(chMode),
+		_mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")) {
 	setAllRadioChannelsToNeutral();
 }
 
@@ -30,4 +40,58 @@ void AP_Controller::setAllRadioChannelsManually() {
 	}
 }
 
+void AP_Controller::update(const float dt) {
+	if (_armingMechanism) _armingMechanism->update(dt);
+
+	// determine flight mode
+	//
+	// check for heartbeat from gcs, if not found go to failsafe
+	if (_hal->heartBeatLost()) {
+		_mode = MAV_MODE_FAILSAFE;
+		_hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
+	// if battery less than 5%, go to failsafe
+	} else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) {
+		_mode = MAV_MODE_FAILSAFE;
+		_hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
+	// manual/auto switch
+	} else {
+		// if all emergencies cleared, fall back to standby
+		if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY);
+		if (_hal->rc[_chMode]->getRadioPosition() > 0) _mode = MAV_MODE_MANUAL;
+		else _mode = MAV_MODE_AUTO;
+	}
+
+	// handle flight modes
+	switch(_mode) {
+
+		case MAV_MODE_LOCKED: {
+			_hal->setState(MAV_STATE_STANDBY);
+			break;
+		}
+
+		case MAV_MODE_FAILSAFE: {
+			_hal->setState(MAV_STATE_EMERGENCY);
+			break;
+		}
+
+		case MAV_MODE_MANUAL: {
+			manualLoop(dt);
+			break;
+		}
+
+		case MAV_MODE_AUTO: {
+			autoLoop(dt);
+			break;
+		}
+
+		default: {
+			_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
+			_hal->setState(MAV_STATE_EMERGENCY);
+		}
+	}
+
+	// this sends commands to motors
+	setMotors();
 }
+
+} // namespace apo

libraries/APO/AP_Controller.h

@@ -22,6 +22,8 @@
 #include <inttypes.h>
 #include "../GCS_MAVLink/GCS_MAVLink.h"
 #include <math.h>
+#include "../AP_Common/AP_Var.h"
+#include "AP_Var_keys.h"
 
 class AP_Var_group;
 
@@ -31,21 +33,38 @@ class AP_HardwareAbstractionLayer;
 class AP_Guide;
 class AP_Navigator;
 class Menu;
+class AP_ArmingMechanism;
 
 /// Controller class
 class AP_Controller {
 public:
 	AP_Controller(AP_Navigator * nav, AP_Guide * guide,
-			AP_HardwareAbstractionLayer * hal);
-	virtual void update(const float & dt) = 0;
-	virtual MAV_MODE getMode() = 0;
-	void setAllRadioChannelsToNeutral();
+			AP_HardwareAbstractionLayer * hal, 
+            AP_ArmingMechanism * armingMechanism,
+            const uint8_t _chMode,
+            const uint16_t key = k_cntrl,
+            const prog_char_t * name = NULL);
+	virtual void update(const float dt);
+	virtual void setMotors() = 0;
+    void setAllRadioChannelsToNeutral();
 	void setAllRadioChannelsManually();
-
+	virtual void manualLoop(const float dt) {
+        setAllRadioChannelsToNeutral();
+    };
+	virtual void autoLoop(const float dt) {
+        setAllRadioChannelsToNeutral();
+    };
+    AP_Uint8 getMode() {
+        return _mode;
+    }
 protected:
 	AP_Navigator * _nav;
 	AP_Guide * _guide;
 	AP_HardwareAbstractionLayer * _hal;
+	AP_ArmingMechanism * _armingMechanism;
+    uint8_t _chMode;
+	AP_Var_group _group;
+	AP_Uint8 _mode;
 };
 
 class AP_ControllerBlock {

libraries/APO/AP_Guide.cpp

@@ -32,13 +32,13 @@ void AP_Guide::setCurrentIndex(uint8_t val){
 }
 
 MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
-		AP_HardwareAbstractionLayer * hal) :
+		AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
 	AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(),
 			_rangeFinderLeft(), _rangeFinderRight(),
 			_group(k_guide, PSTR("guide_")),
-			_velocityCommand(&_group, 1, 1, PSTR("velCmd")),
-			_crossTrackGain(&_group, 2, 1, PSTR("xt")),
-			_crossTrackLim(&_group, 3, 90, PSTR("xtLim")) {
+			_velocityCommand(&_group, 1, velCmd, PSTR("velCmd")),
+			_crossTrackGain(&_group, 2, xt, PSTR("xt")),
+			_crossTrackLim(&_group, 3, xtLim, PSTR("xtLim")) {
 
 	for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) {
 		RangeFinder * rF = _hal->rangeFinders[i];

libraries/APO/AP_Guide.h

@@ -125,7 +125,7 @@ protected:
 class MavlinkGuide: public AP_Guide {
 public:
 	MavlinkGuide(AP_Navigator * navigator,
-			AP_HardwareAbstractionLayer * hal);
+			AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim);
 	virtual void update();
 	void nextCommand();
 	void handleCommand();