Updated APO, HIL working well, live mode ready to test

ArduRover/ArduRover.pde

@@ -15,7 +15,7 @@
 #include <APO.h>
 
 // Vehicle Configuration
-#include "CarStampede.h"
+#include "TankGeneric.h"
 
 // ArduPilotOne Default Setup
 #include "APO_DefaultSetup.h"

ArduRover/CarStampede.h

@@ -12,7 +12,7 @@
 // vehicle options
 static const apo::vehicle_t vehicle = apo::VEHICLE_CAR;
 static const apo::halMode_t halMode = apo::MODE_LIVE;
-static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560;
+static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
 static const uint8_t heartBeatTimeout = 3;
 
 // algorithm selection

ArduRover/TankGeneric.h

@@ -11,7 +11,7 @@
 // vehicle options
 static const apo::vehicle_t vehicle = apo::VEHICLE_TANK;
 static const apo::halMode_t halMode = apo::MODE_LIVE;
-static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560;
+static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
 static const uint8_t heartBeatTimeout = 3;
 
 // algorithm selection
@@ -32,15 +32,15 @@ static const uint8_t heartBeatTimeout = 3;
 
 // optional sensors
 static bool gpsEnabled = false;
-static bool baroEnabled = true;
+static bool baroEnabled = false;
-static bool compassEnabled = true;
+static bool compassEnabled = false;
 
-static bool rangeFinderFrontEnabled = true;
+static bool rangeFinderFrontEnabled = false;
-static bool rangeFinderBackEnabled = true;
+static bool rangeFinderBackEnabled = false;
-static bool rangeFinderLeftEnabled = true;
+static bool rangeFinderLeftEnabled = false;
-static bool rangeFinderRightEnabled = true;
+static bool rangeFinderRightEnabled = false;
-static bool rangeFinderUpEnabled = true;
+static bool rangeFinderUpEnabled = false;
-static bool rangeFinderDownEnabled = true;
+static bool rangeFinderDownEnabled = false;
 
 // loop rates
 static const float loop0Rate = 150;

apo/ControllerPlane.h

@@ -83,7 +83,7 @@ public:
 						pidAltThrLim, pidAltThrDFCut),
 				requireRadio(false) {
 
-		_hal->debug->println_P(PSTR("initializing car controller"));
+		_hal->debug->println_P(PSTR("initializing plane controller"));
 
 		_hal->rc.push_back(
 				new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100,

apo/PlaneEasystar.h

@@ -55,7 +55,7 @@ static const float loop4Rate = 0.1;
 static const float rdrAilMix = 1.0; // since there are no ailerons
 
 // bank error to roll servo
-static const float pidBnkRllP = -0.5;
+static const float pidBnkRllP = -1;
 static const float pidBnkRllI = 0.0;
 static const float pidBnkRllD = 0.0;
 static const float pidBnkRllAwu = 0.0;
@@ -79,7 +79,7 @@ static const float pidSpdPitLim = 1.0;
 static const float pidSpdPitDFCut = 0.0;
 
 // yaw rate error to yaw servo
-static const float pidYwrYawP = -0.2;
+static const float pidYwrYawP = -0.1;
 static const float pidYwrYawI = 0.0;
 static const float pidYwrYawD = 0.0;
 static const float pidYwrYawAwu = 0.0;

libraries/APO/APO_DefaultSetup.h

@@ -18,40 +18,35 @@ void setup() {
 	
 	AP_Var::load_all();
 
+	// Declare all parts of the system
+	AP_Navigator * navigator = NULL;
+	AP_Guide * guide = NULL;
+	AP_Controller * controller = NULL;
+	AP_HardwareAbstractionLayer * hal = NULL;
 	/*
 	 * Communications
 	 */
 	Serial.begin(DEBUG_BAUD, 128, 128); // debug
-	if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs
-	else Serial3.begin(TELEM_BAUD, 128, 128); // gcs
 	
 	// hardware abstraction layer
-	AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer(
+	hal = new AP_HardwareAbstractionLayer(
 			halMode, board, vehicle, heartBeatTimeout);
 	
 	// debug serial
 	hal->debug = &Serial;
 	hal->debug->println_P(PSTR("initializing debug line"));
 
-	/*
-	 * Initialize Comm Channels
-	 */
-	hal->debug->println_P(PSTR("initializing comm channels"));
-	if (hal->getMode() == MODE_LIVE) {
-		Serial1.begin(GPS_BAUD, 128, 16); // gps
-	} else { // hil
-		Serial1.begin(HIL_BAUD, 128, 128);
-	}
-
 	/*
 	 * Sensor initialization
 	 */
 	if (hal->getMode() == MODE_LIVE) {
+
 		hal->debug->println_P(PSTR("initializing adc"));
 		hal->adc = new ADC_CLASS;
 		hal->adc->Init();
 
 		if (gpsEnabled) {
+			Serial1.begin(GPS_BAUD, 128, 16); // gps
 			hal->debug->println_P(PSTR("initializing gps"));
 			AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps));
 			hal->gps = &gpsDriver;
@@ -128,28 +123,45 @@ void setup() {
 			hal->rangeFinders.push_back(rangeFinder);
 		}
 
-	}
+	} else 
 
 	/*
 	 * Select guidance, navigation, control algorithms
 	 */
-	AP_Navigator * navigator = new NAVIGATOR_CLASS(hal);
+	navigator = new NAVIGATOR_CLASS(hal);
-	AP_Guide * guide = new GUIDE_CLASS(navigator, hal);
+	guide = new GUIDE_CLASS(navigator, hal);
-	AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal);
+	controller = new CONTROLLER_CLASS(navigator, guide, hal);
 
 	/*
 	 * CommLinks
 	 */
-	if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal);
+	if (board==BOARD_ARDUPILOTMEGA_2)
-	else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal);
+	{
+		Serial2.begin(TELEM_BAUD, 128, 128); // gcs
+		hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal);
+	}
+	else
+	{
+	   	Serial3.begin(TELEM_BAUD, 128, 128); // gcs
+		hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal);
+	}
+
+	/*
+	 * Hardware in the Loop
+	 */
+	if (hal->getMode() == MODE_HIL_CNTL) {
+		Serial.println("HIL line setting up");
+		Serial1.begin(HIL_BAUD, 128, 128);
+		hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
+	}
 
-	hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
 
 	/*
 	 * Start the autopilot
 	 */
 	hal->debug->printf_P(PSTR("initializing arduplane\n"));
 	hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory());
+
 	autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal,
 			loop0Rate, loop1Rate, loop2Rate, loop3Rate);
 }

libraries/APO/AP_Autopilot.cpp

@@ -29,24 +29,19 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
 	hal->setState(MAV_STATE_CALIBRATING);
 	hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
 	hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
-	navigator->calibrate();
 
-	// start clock
+	if (navigator) navigator->calibrate();
-	//uint32_t timeStart = millis();
-	//uint16_t gpsWaitTime = 5000; // 5 second wait for gps
 
 	/*
 	 * Look for valid initial state
 	 */
-	while (1) {
+	while (_navigator) {
 		// letc gcs known we are alive
 		hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
 		hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
-		hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
-		delay(1000);
 		if (hal->getMode() == MODE_LIVE) {
 			_navigator->updateSlow(0);
-			if (_hal->gps) {
+			if (hal->gps) {
 				if (hal->gps->fix) {
 					break;
 				} else {
@@ -58,7 +53,8 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
 				break;
 			}
 		} else if (hal->getMode() == MODE_HIL_CNTL) { // hil
-			_hal->hil->receive();
+			hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
+			hal->hil->receive();
 			Serial.println("HIL Receive Called");
 			if (_navigator->getTimeStamp() != 0) {
 				// give hil a chance to send some packets
@@ -71,20 +67,24 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
 				break;
 			}
 			hal->debug->println_P(PSTR("waiting for hil packet"));
+			delay(1000);
 		}
 	}
 	
 	AP_MavlinkCommand::home.setAlt(_navigator->getAlt());
 	AP_MavlinkCommand::home.setLat(_navigator->getLat());
 	AP_MavlinkCommand::home.setLon(_navigator->getLon());
+	AP_MavlinkCommand::home.setCommand(MAV_CMD_NAV_WAYPOINT);
 	AP_MavlinkCommand::home.save();
-	_hal->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg\n"),
+	_hal->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg, cmd: %d\n"),
 			AP_MavlinkCommand::home.getLat()*rad2Deg,
-			AP_MavlinkCommand::home.getLon()*rad2Deg);
+			AP_MavlinkCommand::home.getLon()*rad2Deg,
+			AP_MavlinkCommand::home.getCommand());
 	AP_MavlinkCommand::home.load();
-	_hal->debug->printf_P(PSTR("home after load lat: %f deg, lon: %f deg\n"),
+	_hal->debug->printf_P(PSTR("\nhome after load lat: %f deg, lon: %f deg, cmd: %d\n"),
 			AP_MavlinkCommand::home.getLat()*rad2Deg,
-			AP_MavlinkCommand::home.getLon()*rad2Deg);
+			AP_MavlinkCommand::home.getLon()*rad2Deg,
+			AP_MavlinkCommand::home.getCommand());
 
 	/*
 	 * Attach loops
@@ -135,16 +135,7 @@ void AP_Autopilot::callback1(void * data) {
 		apo->getHal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
 	}
 
-	/*
+    /*
-	 * update guidance laws
-	 */
-	if (apo->getGuide())
-	{
-		//apo->getHal()->debug->println_P(PSTR("updating guide"));
-		apo->getGuide()->update();
-	}
-
-	/*
 	 * update control laws
 	 */
 	if (apo->getController()) {
@@ -162,6 +153,15 @@ void AP_Autopilot::callback2(void * data) {
 	AP_Autopilot * apo = (AP_Autopilot *) data;
 	//apo->getHal()->debug->println_P(PSTR("callback 2"));
 	
+	/*
+	 * update guidance laws
+	 */
+	if (apo->getGuide())
+	{
+		//apo->getHal()->debug->println_P(PSTR("updating guide"));
+		apo->getGuide()->update();
+	}
+
 	/*
 	 * send telemetry
 	 */

libraries/APO/AP_CommLink.h

@@ -359,7 +359,7 @@ private:
 		uint32_t timeStamp = micros();
 
 		switch (msg->msgid) {
-		//_hal->debug->printf_P(PSTR("message received: %d"), msg->msgid);
+		_hal->debug->printf_P(PSTR("message received: %d"), msg->msgid);
 
 		case MAVLINK_MSG_ID_HEARTBEAT: {
 			mavlink_heartbeat_t packet;
@@ -497,7 +497,6 @@ private:
 
 			_hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq);
 			AP_MavlinkCommand cmd(packet.seq);
-			cmd.load();
 
 			mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex());
 			mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId,

libraries/APO/AP_Guide.h

@@ -40,7 +40,8 @@ public:
 	 * @param navigator This is the navigator pointer.
 	 */
 	AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) :
-		_navigator(navigator), _hal(hal), _command(0), _previousCommand(0),
+		_navigator(navigator), _hal(hal), _command(AP_MavlinkCommand::home), 
+				_previousCommand(AP_MavlinkCommand::home),
 				_headingCommand(0), _airSpeedCommand(0),
 				_groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0),
 				_pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST),
@@ -144,8 +145,8 @@ public:
 				_rangeFinderLeft(), _rangeFinderRight(),
 				_group(k_guide, PSTR("guide_")),
 				_velocityCommand(&_group, 1, 1, PSTR("velCmd")),
-				_crossTrackGain(&_group, 2, 2, PSTR("xt")),
+				_crossTrackGain(&_group, 2, 1, PSTR("xt")),
-				_crossTrackLim(&_group, 3, 10, PSTR("xtLim")) {
+				_crossTrackLim(&_group, 3, 90, PSTR("xtLim")) {
 
 		for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) {
 			RangeFinder * rF = _hal->rangeFinders[i];
@@ -168,67 +169,8 @@ public:
 	}
 
 	virtual void update() {
-//		_hal->debug->printf_P(
-//				PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"),
-//				getNumberOfCommands(),
-//				getCurrentIndex(),
-//				getPreviousIndex());
-
-		// if we don't have enough waypoint for cross track calcs
-		// go home
-		if (_numberOfCommands == 1) {
-			_mode = MAV_NAV_RETURNING;
-			_altitudeCommand = AP_MavlinkCommand::home.getAlt();
-			_headingCommand = AP_MavlinkCommand::home.bearingTo(
-					_navigator->getLat_degInt(), _navigator->getLon_degInt())
-					+ 180 * deg2Rad;
-			if (_headingCommand > 360 * deg2Rad)
-				_headingCommand -= 360 * deg2Rad;
-
-//			_hal->debug->printf_P(PSTR("going home: bearing: %f distance: %f\n"),
-//			headingCommand,AP_MavlinkCommand::home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt()));
-		} else {
-			_mode = MAV_NAV_WAYPOINT;
-			_altitudeCommand = _command.getAlt();
-			// TODO wrong behavior if 0 selected as waypoint, says previous 0
-			float dXt = _command.crossTrack(_previousCommand,
-					_navigator->getLat_degInt(),
-					_navigator->getLon_degInt());
-			float temp = dXt * _crossTrackGain * deg2Rad; // crosstrack gain, rad/m
-			if (temp > _crossTrackLim * deg2Rad)
-				temp = _crossTrackLim * deg2Rad;
-			if (temp < -_crossTrackLim * deg2Rad)
-				temp = -_crossTrackLim * deg2Rad;
-			float bearing = _previousCommand.bearingTo(_command);
-			_headingCommand = bearing - temp;
-			float alongTrack = _command.alongTrack(_previousCommand,
-					_navigator->getLat_degInt(),
-					_navigator->getLon_degInt());
-			float distanceToNext = _command.distanceTo(
-					_navigator->getLat_degInt(), _navigator->getLon_degInt());
-			float segmentLength = _previousCommand.distanceTo(_command);
-			if (distanceToNext < _command.getRadius() || alongTrack
-					> segmentLength)
-			{
-				Serial.println("radius reached");
-				nextCommand();
-			}
-//			_hal->debug->printf_P(
-//					PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"),
-//					bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack);
-		}
-
-		_groundSpeedCommand = _velocityCommand;
-
-		// TODO : calculate pN,pE,pD from home and gps coordinates
-		_pNCmd = _command.getPN(_navigator->getLat_degInt(),
-				_navigator->getLon_degInt());
-		_pECmd = _command.getPE(_navigator->getLat_degInt(),
-				_navigator->getLon_degInt());
-		_pDCmd = _command.getPD(_navigator->getAlt_intM());
-
 		// process mavlink commands
-		//handleCommand();
+		handleCommand();
 
 		// obstacle avoidance overrides
 		// stop if your going to drive into something in front of you
@@ -237,6 +179,7 @@ public:
 		float frontDistance = _rangeFinderFront->distance / 200.0; //convert for other adc
 		if (_rangeFinderFront && frontDistance < 2) {
 			_mode = MAV_NAV_VECTOR;
+
 			//airSpeedCommand = 0;
 			//groundSpeedCommand = 0;
 //			_headingCommand -= 45 * deg2Rad;
@@ -291,18 +234,74 @@ public:
 		_previousCommand = AP_MavlinkCommand(getPreviousIndex());
 	}
 
-	void handleCommand(AP_MavlinkCommand command,
+	void handleCommand() {
-			AP_MavlinkCommand previousCommand) {
+
 		// TODO handle more commands
-		switch (command.getCommand()) {
+		switch (_command.getCommand()) {
+
 		case MAV_CMD_NAV_WAYPOINT: {
-			// if within radius, increment
+
-			float d = previousCommand.distanceTo(_navigator->getLat_degInt(),
+			// if we don't have enough waypoint for cross track calcs
-					_navigator->getLon_degInt());
+		    // go home
-			if (d < command.getRadius()) {
+			if (_numberOfCommands == 1) {
-				nextCommand();
+				_mode = MAV_NAV_RETURNING;
-				Serial.println("radius reached");
+				_altitudeCommand = AP_MavlinkCommand::home.getAlt();
+				_headingCommand = AP_MavlinkCommand::home.bearingTo(
+						_navigator->getLat_degInt(), _navigator->getLon_degInt())
+						+ 180 * deg2Rad;
+				if (_headingCommand > 360 * deg2Rad)
+					_headingCommand -= 360 * deg2Rad;
+
+				//_hal->debug->printf_P(PSTR("going home: bearing: %f distance: %f\n"),
+				//headingCommand,AP_MavlinkCommand::home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt()));
+				
+			// if we have 2 or more waypoints do x track navigation
+			} else {
+				_mode = MAV_NAV_WAYPOINT;
+				float alongTrack = _command.alongTrack(_previousCommand,
+						_navigator->getLat_degInt(),
+						_navigator->getLon_degInt());
+				float distanceToNext = _command.distanceTo(
+						_navigator->getLat_degInt(), _navigator->getLon_degInt());
+				float segmentLength = _previousCommand.distanceTo(_command);
+				if (distanceToNext < _command.getRadius() || alongTrack
+						> segmentLength)
+				{
+					Serial.println("waypoint reached");
+					nextCommand();
+				}
+				_altitudeCommand = _command.getAlt();
+				float dXt = _command.crossTrack(_previousCommand,
+						_navigator->getLat_degInt(),
+						_navigator->getLon_degInt());
+				float temp = dXt * _crossTrackGain * deg2Rad; // crosstrack gain, rad/m
+				if (temp > _crossTrackLim * deg2Rad)
+					temp = _crossTrackLim * deg2Rad;
+				if (temp < -_crossTrackLim * deg2Rad)
+					temp = -_crossTrackLim * deg2Rad;
+				float bearing = _previousCommand.bearingTo(_command);
+				_headingCommand = bearing - temp;
+				//_hal->debug->printf_P(
+						//PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"),
+						//bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack);
 			}
+
+			_groundSpeedCommand = _velocityCommand;
+
+			// calculate pN,pE,pD from home and gps coordinates
+			_pNCmd = _command.getPN(_navigator->getLat_degInt(),
+					_navigator->getLon_degInt());
+			_pECmd = _command.getPE(_navigator->getLat_degInt(),
+					_navigator->getLon_degInt());
+			_pDCmd = _command.getPD(_navigator->getAlt_intM());
+
+			// debug 
+			//_hal->debug->printf_P(
+				//PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"),
+				//getNumberOfCommands(),
+				//getCurrentIndex(),
+				//getPreviousIndex());
+
 			break;
 		}
 //		case MAV_CMD_CONDITION_CHANGE_ALT:
@@ -354,4 +353,5 @@ private:
 } // namespace apo
 
 #endif // AP_Guide_H
+
 // vim:ts=4:sw=4:expandtab

libraries/APO/AP_MavlinkCommand.cpp

@@ -24,6 +24,19 @@ AP_MavlinkCommand::AP_MavlinkCommand(uint16_t index, bool doLoad) :
 		Serial.print("key: "); Serial.println(k_commands + index);
 		Serial.println("++");
 	}
+
+	// default values for structure
+	_data.get().command = MAV_CMD_NAV_WAYPOINT;
+	_data.get().autocontinue = true;
+	_data.get().frame = MAV_FRAME_GLOBAL;
+	_data.get().param1 = 0;
+	_data.get().param2 = 10; // radius of 10 meters
+	_data.get().param3 = 0;
+	_data.get().param4 = 0;
+	_data.get().x = 0;
+	_data.get().y = 0;
+	_data.get().z = 1000;
+
 	// This is a failsafe measure to stop trying to load a command if it can't load
 	if (doLoad && !load()) {
 		Serial.println("load failed, reverting to home waypoint");
@@ -153,11 +166,10 @@ float AP_MavlinkCommand::distanceTo(int32_t lat_degInt, int32_t lon_degInt) cons
 //calculates cross track of a current location
 float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous,
 		int32_t lat_degInt, int32_t lon_degInt) const {
-		float d = previous.distanceTo(lat_degInt, lon_degInt);
+	float d = previous.distanceTo(lat_degInt, lon_degInt);
-		float bCurrent = previous.bearingTo(lat_degInt, lon_degInt);
+	float bCurrent = previous.bearingTo(lat_degInt, lon_degInt);
-		float bNext = previous.bearingTo(*this);
+	float bNext = previous.bearingTo(*this);
-		return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth;
+	return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth;
-	return 0;
 }
 
 // calculates along  track distance of a current location

libraries/APO/AP_Navigator.h

@@ -273,13 +273,9 @@ public:
 
 		AP_Navigator::calibrate();
 
-		// TODO: handle cold restart
+		// TODO: handle cold/warm restart
 		if (_hal->imu) {
-			/*
+			_hal->imu->init(IMU::COLD_START,delay);
-			 * Gyro has built in warm up cycle and should
-			 * run first */
-			_hal->imu->init_gyro(NULL);
-			_hal->imu->init_accel(NULL);
 		}
 	}
 	virtual void updateFast(float dt) {