VTOL path fixes

Flight mode fixes
POI mode
AutoLand Tests
Simple stabilization Shortest way yaw fix
GCS map and pathplanner fixes

flight/Libraries/CoordinateConversions.c

@@ -148,9 +148,9 @@ void RPY2Quaternion(const float rpy[3], float q[4])
 	float phi, theta, psi;
 	float cphi, sphi, ctheta, stheta, cpsi, spsi;
 
-	phi = DEG2RAD * rpy[0] / 2;
+	phi = DEG2RAD * rpy[0] / 2.0f;
-	theta = DEG2RAD * rpy[1] / 2;
+	theta = DEG2RAD * rpy[1] / 2.0f;
-	psi = DEG2RAD * rpy[2] / 2;
+	psi = DEG2RAD * rpy[2] / 2.0f;
 	cphi = cosf(phi);
 	sphi = sinf(phi);
 	ctheta = cosf(theta);

flight/Libraries/paths.c

@@ -176,6 +176,8 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
 	float radius_north, radius_east, diff_north, diff_east;
 	float radius,cradius;
 	float normal[2];
+	float progress;
+	float a_diff, a_radius;
 
 	// Radius
 	radius_north = end_point[0] - start_point[0];
@@ -185,8 +187,8 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
 	diff_north = cur_point[0] - end_point[0];
 	diff_east = cur_point[1] - end_point[1];
 
-	radius = sqrtf( radius_north * radius_north + radius_east * radius_east );
+	radius = sqrtf( powf(radius_north,2) + powf(radius_east,2) );
-	cradius = sqrtf(  diff_north * diff_north   +   diff_east * diff_east );
+	cradius = sqrtf( powf(diff_north,2) + powf(diff_east,2) );
 
 	if (cradius < 1e-6) {
 		// cradius is zero, just fly somewhere and make sure correction is still a normal
@@ -209,7 +211,28 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
 		normal[1] = -diff_north / cradius;
 	}
 	
-	status->fractional_progress = (clockwise?1:-1) * atan2f( diff_north, diff_east) - atan2f( radius_north, radius_east);
+
+	// normalize progress to 0..1
+	a_diff = atan2f( diff_north, diff_east);
+	a_radius = atan2f( radius_north, radius_east);
+	
+	if(a_diff<0)
+		a_diff+=2*M_PI;
+	if(a_radius<0)
+		a_radius+=2*M_PI;
+		
+	progress = (a_diff - a_radius + M_PI) / (2 * M_PI);
+
+	if(progress<0)
+		progress+=1.0;
+	else if(progress>=1)
+		progress-=1.0;
+
+	if(clockwise)
+	{
+		progress=1-progress;
+	}
+	status->fractional_progress = progress;
 
 	// error is current radius minus wanted radius - positive if too close
 	status->error = radius - cradius;

flight/Modules/ManualControl/manualcontrol.c

@@ -93,6 +93,7 @@ static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
 // Private functions
 static void updateActuatorDesired(ManualControlCommandData * cmd);
 static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
+static void updateLandDesired(ManualControlCommandData * cmd, bool changed);
 static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed);
 static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home);
 static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
@@ -455,11 +456,18 @@ static void manualControlTask(void *parameters)
 						altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
 						break;
 					case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+					case FLIGHTSTATUS_FLIGHTMODE_POI:
 						updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
 						break;
 					case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
 						updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
 						break;
+					case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
+						// No need to call anything.  This just avoids errors.
+						break;
+					case FLIGHTSTATUS_FLIGHTMODE_LAND:
+						updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
+						break;
 					default:
 						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
 				}
@@ -728,12 +736,12 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
 		
 		PathDesiredData pathDesired;
 		PathDesiredGet(&pathDesired);
-		pathDesired.Start[PATHDESIRED_END_NORTH] = positionActual.North;
+		pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
-		pathDesired.Start[PATHDESIRED_END_EAST] = positionActual.East;
+		pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
-		pathDesired.Start[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
+		pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
 		pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
 		pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
-		pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
+		pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
 		pathDesired.StartingVelocity=1;
 		pathDesired.EndingVelocity=0;
 		pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@@ -751,6 +759,38 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
 	}
 }
 
+
+static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
+{
+	static portTickType lastSysTime;
+	portTickType thisSysTime;
+	float dT;
+
+	thisSysTime = xTaskGetTickCount();
+	dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
+	lastSysTime = thisSysTime;
+
+	PositionActualData positionActual;
+	PositionActualGet(&positionActual);
+
+	PathDesiredData pathDesired;
+	PathDesiredGet(&pathDesired);
+	if(changed) {
+		// After not being in this mode for a while init at current height
+		pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
+		pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
+		pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
+		pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
+		pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
+		pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
+		pathDesired.StartingVelocity=1;
+		pathDesired.EndingVelocity=0;
+		pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+	}
+	pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5;
+	PathDesiredSet(&pathDesired);
+}
+
 /**
  * @brief Update the altitude desired to current altitude when
  * enabled and enable altitude mode for stabilization
@@ -804,6 +844,11 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool
 	AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
 }
 
+static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
+{
+	AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
+}
+
 static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
 {
 	AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);

flight/Modules/PathPlanner/pathplanner.c

@@ -238,20 +238,25 @@ void updatePathDesired(UAVObjEvent * ev) {
 	pathDesired.UID = waypointActive.Index;
 
 	if(waypointActive.Index == 0) {
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
 		// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
 
-		pathDesired.Start[PATHDESIRED_START_NORTH] =  waypoint.Position[WAYPOINT_POSITION_NORTH];
+		/*pathDesired.Start[PATHDESIRED_START_NORTH] =  waypoint.Position[WAYPOINT_POSITION_NORTH];
 		pathDesired.Start[PATHDESIRED_START_EAST] =  waypoint.Position[WAYPOINT_POSITION_EAST];
-		pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];
+		pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];*/
+		pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
+		pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
+		pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
 		pathDesired.StartingVelocity = pathDesired.EndingVelocity;
 	} else {
 		// Get previous waypoint as start point
 		WaypointData waypointPrev;
 		WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
 
-		pathDesired.Start[PATHDESIRED_END_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
+		pathDesired.Start[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
-		pathDesired.Start[PATHDESIRED_END_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
+		pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
-		pathDesired.Start[PATHDESIRED_END_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
+		pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
 		pathDesired.StartingVelocity = waypointPrev.Velocity;
 	}
 	PathDesiredSet(&pathDesired);

flight/Modules/Stabilization/inc/relay_tuning.h

@@ -36,4 +36,4 @@
 
 int stabilization_relay_rate(float err, float *output, int axis, bool reinit);
 
-#endif
+#endif

flight/Modules/Stabilization/relay_tuning.c

@@ -32,15 +32,11 @@
  */
 
 #include "openpilot.h"
+#include "stabilization.h"
 #include "relaytuning.h"
 #include "relaytuningsettings.h"
 #include "sin_lookup.h"
 
-//! Private variables
-static const int SIN_RESOLUTION = 180;
-
-#define MAX_AXES 3
-
 /**
  * Apply a step function for the stabilization controller and monitor the 
  * result
@@ -108,7 +104,7 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
 	if(phase >= 360)
 		phase = 0;
 	accum_sin += sin_lookup_deg(phase) * error;
-	accum_cos += sin_lookup_deg(phase + 90) * error;
+	accum_cos += cos_lookup_deg(phase) * error;
 	accumulated ++;
 
 	// Make sure we've had enough time since last transition then check for a change in the output
@@ -148,4 +144,3 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
 	return 0;
 }
 
-

flight/Modules/Stabilization/stabilization.c

@@ -53,6 +53,9 @@
 #include "relay_tuning.h"
 #include "virtualflybar.h"
 
+// Includes for various stabilization algorithms
+#include "relay_tuning.h"
+
 // Private constants
 #define MAX_QUEUE_SIZE 1
 
@@ -123,7 +126,6 @@ int32_t StabilizationInitialize()
 #ifdef DIAG_RATEDESIRED
 	RateDesiredInitialize();
 #endif
-
 	// Code required for relay tuning
 	sin_lookup_initalize();
 	RelayTuningSettingsInitialize();
@@ -211,7 +213,12 @@ static void stabilizationTask(void* parameters)
 		float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
 			stabDesired.Pitch - attitudeActual.Pitch,
 			stabDesired.Yaw - attitudeActual.Yaw};
-		local_error[2] = fmodf(local_error[2] + 180, 360) - 180;
+		// find shortest way
+		float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
+		if(modulo<0)
+			local_error[2] = modulo + 180.0f;
+		else
+			local_error[2] = modulo - 180.0f;
 #endif
 
 		float gyro_filtered[3];
@@ -277,6 +284,7 @@ static void stabilizationTask(void* parameters)
 					stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings);
 
 					break;
+
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
 				{
 					if (reinit)
@@ -292,6 +300,7 @@ static void stabilizationTask(void* parameters)
 
 					break;
 				}
+
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
 					if (reinit)
 						pids[PID_RATE_ROLL + i].iAccumulator = 0;
@@ -307,7 +316,7 @@ static void stabilizationTask(void* parameters)
 						rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
 					}
 
-					rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
+					rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[i]);
 
 					actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i],  gyro_filtered[i], dT);
 					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);

flight/Modules/VtolPathFollower/vtolpathfollower.c

@@ -70,30 +70,40 @@
 #include "velocityactual.h"
 #include "CoordinateConversions.h"
 
+#include "cameradesired.h"
+#include "poilearnsettings.h"
+#include "poilocation.h"
+#include "accessorydesired.h"
+
+
 // Private constants
 #define MAX_QUEUE_SIZE 4
 #define STACK_SIZE_BYTES 1548
 #define TASK_PRIORITY (tskIDLE_PRIORITY+2)
 #define F_PI 3.14159265358979323846f
 #define DEG2RAD (F_PI/180.0f)
+#define RAD2DEG(rad) ((rad)*(180.0f/F_PI))
 
 // Private types
 
 // Private variables
 static xTaskHandle pathfollowerTaskHandle;
 static PathDesiredData pathDesired;
+static PathStatusData pathStatus;
 static VtolPathFollowerSettingsData vtolpathfollowerSettings;
 
 // Private functions
 static void vtolPathFollowerTask(void *parameters);
 static void SettingsUpdatedCb(UAVObjEvent * ev);
 static void updateNedAccel();
+static void updatePOIBearing();
 static void updatePathVelocity();
 static void updateEndpointVelocity();
 static void updateFixedAttitude(float* attitude);
-static void updateVtolDesiredAttitude();
+static void updateVtolDesiredAttitude(bool yaw_attitude);
 static float bound(float val, float min, float max);
 static bool vtolpathfollower_enabled;
+static void accessoryUpdated(UAVObjEvent* ev);
 
 /**
  * Initialise the module, called on startup
@@ -126,6 +136,10 @@ int32_t VtolPathFollowerInitialize()
 		PathDesiredInitialize();
 		PathStatusInitialize();
 		VelocityDesiredInitialize();
+		CameraDesiredInitialize();
+		AccessoryDesiredInitialize();
+		PoiLearnSettingsInitialize();
+		PoiLocationInitialize();
 		vtolpathfollower_enabled = true;
 	} else {
 		vtolpathfollower_enabled = false;
@@ -152,12 +166,12 @@ static void vtolPathFollowerTask(void *parameters)
 {
 	SystemSettingsData systemSettings;
 	FlightStatusData flightStatus;
-	PathStatusData pathStatus;
 
 	portTickType lastUpdateTime;
 	
 	VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
 	PathDesiredConnectCallback(SettingsUpdatedCb);
+	AccessoryDesiredConnectCallback(accessoryUpdated);
 	
 	VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
 	PathDesiredGet(&pathDesired);
@@ -200,11 +214,12 @@ static void vtolPathFollowerTask(void *parameters)
 
 		// Check the combinations of flightmode and pathdesired mode
 		switch(flightStatus.FlightMode) {
+			case FLIGHTSTATUS_FLIGHTMODE_LAND:
 			case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
 			case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
 				if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
 					updateEndpointVelocity();
-					updateVtolDesiredAttitude();
+					updateVtolDesiredAttitude(false);
 					AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
 				} else {
 					AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
@@ -216,15 +231,11 @@ static void vtolPathFollowerTask(void *parameters)
 				switch(pathDesired.Mode) {
 					// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
 					case PATHDESIRED_MODE_FLYENDPOINT:
-						updateEndpointVelocity();
-						updateVtolDesiredAttitude();
-						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
-						break;
 					case PATHDESIRED_MODE_FLYVECTOR:
 					case PATHDESIRED_MODE_FLYCIRCLERIGHT:
 					case PATHDESIRED_MODE_FLYCIRCLELEFT:
 						updatePathVelocity();
-						updateVtolDesiredAttitude();
+						updateVtolDesiredAttitude(false);
 						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
 						break;
 					case PATHDESIRED_MODE_FIXEDATTITUDE:
@@ -239,6 +250,16 @@ static void vtolPathFollowerTask(void *parameters)
 						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
 						break;
 				}
+				PathStatusSet(&pathStatus);
+				break;
+			case FLIGHTSTATUS_FLIGHTMODE_POI:
+				if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+					updateEndpointVelocity();
+					updateVtolDesiredAttitude(true);
+					updatePOIBearing();
+				} else {
+					AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
+				}
 				break;
 			default:
 				// Be cleaner and get rid of global variables
@@ -262,6 +283,54 @@ static void vtolPathFollowerTask(void *parameters)
 	}
 }
 
+/**
+ * Compute bearing and elevation between current position and POI
+ */
+static void updatePOIBearing()
+{
+	PositionActualData positionActual;
+	PositionActualGet(&positionActual);
+	CameraDesiredData cameraDesired;
+	CameraDesiredGet(&cameraDesired);
+	StabilizationDesiredData stabDesired;
+	StabilizationDesiredGet(&stabDesired);
+	PoiLocationData poi;
+	PoiLocationGet(&poi);
+	//use poi here
+	//HomeLocationData poi;
+	//HomeLocationGet (&poi);
+
+	float dLoc[3];
+	float yaw=0;
+	float elevation=0;
+
+	dLoc[0]=positionActual.North-poi.North;
+	dLoc[1]=positionActual.East-poi.East;
+	dLoc[2]=positionActual.Down-poi.Down;
+
+	if(dLoc[1]<0)
+		yaw=RAD2DEG(atan2f(dLoc[1],dLoc[0]))+180;
+	else
+		yaw=RAD2DEG(atan2f(dLoc[1],dLoc[0]))-180;
+
+	// distance
+	float distance = sqrtf(powf(dLoc[0],2)+powf(dLoc[1],2));
+
+	//not above
+	if(distance!=0) {
+		//You can feed this into camerastabilization
+		elevation=RAD2DEG(atan2f(dLoc[2],distance));
+	}
+	stabDesired.Yaw=yaw;
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	cameraDesired.Yaw=yaw;
+	cameraDesired.PitchOrServo2=elevation;
+
+	CameraDesiredSet(&cameraDesired);
+	StabilizationDesiredSet(&stabDesired);
+}
+
+
 /**
  * Compute desired velocity from the current position and path
  *
@@ -280,11 +349,31 @@ static void updatePathVelocity()
 	struct path_status progress;
 	
 	path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
-	
+
-	float groundspeed = pathDesired.StartingVelocity + 
+	float groundspeed;
-	    (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound ( progress.fractional_progress,0,1);
+	switch (pathDesired.Mode) {
-	if(progress.fractional_progress > 1)
+		case PATHDESIRED_MODE_FLYCIRCLERIGHT:
-		groundspeed = 0;
+		case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
+		case PATHDESIRED_MODE_FLYCIRCLELEFT:
+		case PATHDESIRED_MODE_DRIVECIRCLELEFT:
+			groundspeed = pathDesired.EndingVelocity;
+			break;
+		case PATHDESIRED_MODE_FLYENDPOINT:
+		case PATHDESIRED_MODE_DRIVEENDPOINT:
+			groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity *
+				bound(progress.fractional_progress,0,1);
+			if(progress.fractional_progress > 1)
+				groundspeed = 0;
+			break;
+		case PATHDESIRED_MODE_FLYVECTOR:
+		case PATHDESIRED_MODE_DRIVEVECTOR:
+		default:
+			groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
+				bound(progress.fractional_progress,0,1);
+			if(progress.fractional_progress > 1)
+				groundspeed = 0;
+			break;
+	}
 	
 	VelocityDesiredData velocityDesired;
 	velocityDesired.North = progress.path_direction[0] * groundspeed;
@@ -314,6 +403,10 @@ static void updatePathVelocity()
 								 -vtolpathfollowerSettings.VerticalVelMax,
 								 vtolpathfollowerSettings.VerticalVelMax);
 
+	// update pathstatus
+	pathStatus.error = progress.error;
+	pathStatus.fractional_progress = progress.fractional_progress;
+
 	VelocityDesiredSet(&velocityDesired);
 }
 
@@ -332,7 +425,7 @@ void updateEndpointVelocity()
 	
 	PositionActualGet(&positionActual);
 	VelocityDesiredGet(&velocityDesired);
-	
+
 	float northError;
 	float eastError;
 	float downError;
@@ -435,7 +528,7 @@ static void updateFixedAttitude(float* attitude)
  * NED frame as the feedback term and then compares the 
  * @ref VelocityActual against the @ref VelocityDesired
  */
-static void updateVtolDesiredAttitude()
+static void updateVtolDesiredAttitude(bool yaw_attitude)
 {
 	float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
 
@@ -501,7 +594,6 @@ static void updateVtolDesiredAttitude()
 	// Testing code - refactor into manual control command
 	ManualControlCommandData manualControlData;
 	ManualControlCommandGet(&manualControlData);
-	stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;	
 	
 	// Compute desired north command
 	northError = velocityDesired.North - northVel;
@@ -554,8 +646,12 @@ static void updateVtolDesiredAttitude()
 	
 	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+	if(yaw_attitude) {
-	
+		stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	} else {
+		stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+		stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;
+	}
 	StabilizationDesiredSet(&stabDesired);
 }
 
@@ -618,3 +714,28 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
 	PathDesiredGet(&pathDesired);
 }
 
+static void accessoryUpdated(UAVObjEvent* ev)
+{
+	if (ev->obj != AccessoryDesiredHandle())
+		return;
+
+	PositionActualData positionActual;
+	PositionActualGet(&positionActual);
+	AccessoryDesiredData accessory;
+	PoiLearnSettingsData poiLearn;
+	PoiLearnSettingsGet(&poiLearn);
+	PoiLocationData poi;
+	PoiLocationGet(&poi);
+	if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) {
+		if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
+			if(accessory.AccessoryVal<-0.5f)
+			{
+				poi.North=positionActual.North;
+				poi.East=positionActual.East;
+				poi.Down=positionActual.Down;
+				PoiLocationSet(&poi);
+			}
+		}
+	}
+}
+

flight/targets/RevoMini/UAVObjects.inc

@@ -89,6 +89,8 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
 UAVOBJSRCFILENAMES += altitudeholddesired
 UAVOBJSRCFILENAMES += waypoint
 UAVOBJSRCFILENAMES += waypointactive
+UAVOBJSRCFILENAMES += poilocation
+UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 UAVOBJSRCFILENAMES += txpidsettings
 

flight/targets/Revolution/UAVObjects.inc

@@ -94,6 +94,8 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
 UAVOBJSRCFILENAMES += altitudeholddesired
 UAVOBJSRCFILENAMES += waypoint
 UAVOBJSRCFILENAMES += waypointactive
+UAVOBJSRCFILENAMES += poilocation
+UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )

ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp

@@ -28,7 +28,7 @@
 #include "gpsitem.h"
 namespace mapcontrol
 {
-    GPSItem::GPSItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(5),traildistance(50),autosetreached(true)
+    GPSItem::GPSItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(1),traildistance(2),autosetreached(true)
     ,autosetdistance(100)
     {
         pic.load(uavPic);
@@ -42,8 +42,10 @@ namespace mapcontrol
         trailLine=new QGraphicsItemGroup(this);
         trailLine->setParentItem(map);
         this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true);
+        setCacheMode(QGraphicsItem::ItemCoordinateCache);
         mapfollowtype=UAVMapFollowType::None;
-        trailtype=UAVTrailType::ByDistance;
+        //trailtype=UAVTrailType::ByDistance;
+        trailtype=UAVTrailType::ByTimeElapsed;
         timer.start();
         connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos()));
         connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal)));
@@ -74,12 +76,12 @@ namespace mapcontrol
             {
                 if(timer.elapsed()>trailtime*1000)
                 {
-                    TrailItem * ob=new TrailItem(position,altitude,Qt::green,map);
+                    TrailItem * ob=new TrailItem(position,altitude,Qt::red,map);
                     trail->addToGroup(ob);
                     connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT()));
                     if(!lasttrailline.IsEmpty())
                     {
-                        TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map);
+                        TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map);
                         trailLine->addToGroup(obj);
                         connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot()));
                     }
@@ -92,12 +94,12 @@ namespace mapcontrol
             {
                 if(qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord,position)*1000)>traildistance)
                 {
-                    TrailItem * ob=new TrailItem(position,altitude,Qt::green,map);
+                    TrailItem * ob=new TrailItem(position,altitude,Qt::red,map);
                     trail->addToGroup(ob);
                     connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT()));
                     if(!lasttrailline.IsEmpty())
                     {
-                        TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map);
+                        TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map);
                         trailLine->addToGroup(obj);
                         connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot()));
                     }

ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp

@@ -80,6 +80,7 @@ namespace mapcontrol
 
             if(GPS!=0)
             {
+                GPS->DeleteTrail();
                 delete GPS;
                 GPS=0;
             }
@@ -93,6 +94,7 @@ namespace mapcontrol
             {
                 GPS=new GPSItem(map,this);
                 GPS->setParentItem(map);
+                GPS->setOpacity(overlayOpacity);
                 setOverlayOpacity(overlayOpacity);
             }
         }

ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp

@@ -200,6 +200,17 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets()
         ui->dsb_condParam4->setVisible(false);
         ui->condParam1->setText("Relative Distance(0=complete,1=just starting)");
         break;
+    case MapDataDelegate::ENDCONDITION_BELOWERROR:
+        ui->condParam1->setVisible(true);
+        ui->condParam2->setVisible(false);
+        ui->condParam3->setVisible(false);
+        ui->condParam4->setVisible(false);
+        ui->dsb_condParam1->setVisible(true);
+        ui->dsb_condParam2->setVisible(false);
+        ui->dsb_condParam3->setVisible(false);
+        ui->dsb_condParam4->setVisible(false);
+        ui->condParam1->setText("error margin (in m)");
+        break;
     case MapDataDelegate::ENDCONDITION_ABOVEALTITUDE:
         ui->condParam1->setVisible(true);
         ui->condParam2->setVisible(false);
@@ -211,6 +222,18 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets()
         ui->dsb_condParam4->setVisible(false);
         ui->condParam1->setText("Altitude in meters (negative)");
         break;
+    case MapDataDelegate::ENDCONDITION_ABOVESPEED:
+        ui->condParam1->setVisible(true);
+        ui->condParam2->setVisible(true);
+        ui->condParam3->setVisible(false);
+        ui->condParam4->setVisible(false);
+        ui->dsb_condParam1->setVisible(true);
+        ui->dsb_condParam2->setVisible(true);
+        ui->dsb_condParam3->setVisible(false);
+        ui->dsb_condParam4->setVisible(false);
+        ui->condParam1->setText("Speed in meters/second");
+        ui->condParam2->setText("flag: 0=groundspeed 1=airspeed");
+        break;
     case MapDataDelegate::ENDCONDITION_POINTINGTOWARDSNEXT:
         ui->condParam1->setVisible(true);
         ui->condParam2->setVisible(false);

ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp

@@ -24,6 +24,7 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+#define USE_PATHPLANNER
 
 #include "opmapgadgetwidget.h"
 #include "ui_opmap_widget.h"
@@ -546,8 +547,8 @@ void OPMapGadgetWidget::closeEvent(QCloseEvent *event)
 */
 void OPMapGadgetWidget::updatePosition()
 {
-	double uav_latitude, uav_longitude, uav_altitude, uav_yaw;
+    double uav_latitude, uav_longitude, uav_altitude, uav_yaw;
-	double gps_latitude, gps_longitude, gps_altitude, gps_heading;
+    double gps_latitude, gps_longitude, gps_altitude, gps_heading;
 
 	internals::PointLatLng uav_pos;
 	internals::PointLatLng gps_pos;
@@ -581,7 +582,7 @@ void OPMapGadgetWidget::updatePosition()
     gps_longitude = gpsPositionData.Longitude;
     gps_altitude = gpsPositionData.Altitude;
 
-	gps_pos = internals::PointLatLng(gps_latitude, gps_longitude);
+    gps_pos = internals::PointLatLng(gps_latitude*1e-7, gps_longitude*1e-7);
 
     //**********************
     // get the current position and heading estimates
@@ -643,6 +644,7 @@ void OPMapGadgetWidget::updatePosition()
     {
         m_map->GPS->SetUAVPos(gps_pos, gps_altitude); // set the maps GPS position
         m_map->GPS->SetUAVHeading(gps_heading);       // set the maps GPS heading
+        m_map->GPS->update();
     }
     m_map->UAV->updateTextOverlay();
     m_map->UAV->update();

ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp

@@ -121,7 +121,9 @@ void MapDataDelegate::loadComboBox(QComboBox *combo, flightDataModel::pathPlanDa
         combo->addItem("Timeout",ENDCONDITION_TIMEOUT);
         combo->addItem("Distance to tgt",ENDCONDITION_DISTANCETOTARGET);
         combo->addItem("Leg remaining",ENDCONDITION_LEGREMAINING);
+        combo->addItem("Below Error",ENDCONDITION_BELOWERROR);
         combo->addItem("Above Altitude",ENDCONDITION_ABOVEALTITUDE);
+        combo->addItem("Above Speed",ENDCONDITION_ABOVESPEED);
         combo->addItem("Pointing towards next",ENDCONDITION_POINTINGTOWARDSNEXT);
         combo->addItem("Python script",ENDCONDITION_PYTHONSCRIPT);
         combo->addItem("Immediate",ENDCONDITION_IMMEDIATE);

ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h

@@ -41,8 +41,9 @@ class MapDataDelegate : public QItemDelegate
                    MODE_DRIVEENDPOINT=4, MODE_DRIVEVECTOR=5, MODE_DRIVECIRCLELEFT=6, MODE_DRIVECIRCLERIGHT=7,
                    MODE_FIXEDATTITUDE=8, MODE_SETACCESSORY=9, MODE_DISARMALARM=10 } ModeOptions;
     typedef enum { ENDCONDITION_NONE=0, ENDCONDITION_TIMEOUT=1, ENDCONDITION_DISTANCETOTARGET=2,
-                   ENDCONDITION_LEGREMAINING=3, ENDCONDITION_ABOVEALTITUDE=4, ENDCONDITION_POINTINGTOWARDSNEXT=5,
+                   ENDCONDITION_LEGREMAINING=3, ENDCONDITION_BELOWERROR=4, ENDCONDITION_ABOVEALTITUDE=5,
-                   ENDCONDITION_PYTHONSCRIPT=6, ENDCONDITION_IMMEDIATE=7 } EndConditionOptions;
+                   ENDCONDITION_ABOVESPEED=6, ENDCONDITION_POINTINGTOWARDSNEXT=7, ENDCONDITION_PYTHONSCRIPT=8,
+                   ENDCONDITION_IMMEDIATE=9 } EndConditionOptions;
     typedef enum { COMMAND_ONCONDITIONNEXTWAYPOINT=0, COMMAND_ONNOTCONDITIONNEXTWAYPOINT=1,
                    COMMAND_ONCONDITIONJUMPWAYPOINT=2, COMMAND_ONNOTCONDITIONJUMPWAYPOINT=3,
                    COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT=4 } CommandOptions;

ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

@@ -95,6 +95,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/txpidsettings.h \
     $$UAVOBJECT_SYNTHETICS/cameradesired.h \
     $$UAVOBJECT_SYNTHETICS/faultsettings.h \
+    $$UAVOBJECT_SYNTHETICS/poilearnsettings.h \
+    $$UAVOBJECT_SYNTHETICS/poilocation.h \
     $$UAVOBJECT_SYNTHETICS/oplinksettings.h \
     $$UAVOBJECT_SYNTHETICS/oplinkstatus.h \
     $$UAVOBJECT_SYNTHETICS/osdsettings.h \
@@ -175,6 +177,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/txpidsettings.cpp \
     $$UAVOBJECT_SYNTHETICS/cameradesired.cpp \
     $$UAVOBJECT_SYNTHETICS/faultsettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/poilearnsettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/poilocation.cpp \
     $$UAVOBJECT_SYNTHETICS/oplinksettings.cpp \
     $$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \

shared/uavobjectdefinition/poilearnsettings.xml

@@ -0,0 +1,10 @@
+<xml>
+    <object name="PoiLearnSettings" singleinstance="true" settings="true">
+        <description>Settings for the @ref Point of Interest feature</description>
+        <field name="Input" units="channel" type="enum" elements="1" options="Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5,None" defaultvalue="None"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+        <logging updatemode="manual" period="0"/>
+	</object>
+</xml>

shared/uavobjectdefinition/poilocation.xml

@@ -0,0 +1,12 @@
+<xml>
+    <object name="PoiLocation" singleinstance="true" settings="false">
+        <description>Contains the current Point of interest relative to @ref HomeLocation</description>
+        <field name="North" units="m" type="float" elements="1"/>
+        <field name="East" units="m" type="float" elements="1"/>
+        <field name="Down" units="m" type="float" elements="1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+        <logging updatemode="periodic" period="1000"/>
+    </object>
+</xml>