Enable guidance in revo. Update manual control to set the position desired

when the switch is flipped.

flight/Modules/Guidance/guidance.c

@@ -45,13 +45,13 @@
 
 #include "openpilot.h"
 #include "guidance.h"
-#include "guidancesettings.h"
+#include "accels.h"
-#include "attituderaw.h"
 #include "attitudeactual.h"
 #include "positiondesired.h"	// object that will be updated by the module
 #include "positionactual.h"
 #include "manualcontrol.h"
 #include "flightstatus.h"
+#include "guidancesettings.h"
 #include "nedaccel.h"
 #include "stabilizationdesired.h"
 #include "stabilizationsettings.h"
@@ -61,7 +61,7 @@
 #include "CoordinateConversions.h"
 
 // Private constants
-#define MAX_QUEUE_SIZE 1
+#define MAX_QUEUE_SIZE 4
 #define STACK_SIZE_BYTES 1024
 #define TASK_PRIORITY (tskIDLE_PRIORITY+2)
 // Private types
@@ -97,7 +97,6 @@ int32_t GuidanceStart()
  */
 int32_t GuidanceInitialize()
 {
-
 	GuidanceSettingsInitialize();
 	PositionDesiredInitialize();
 	NedAccelInitialize();
@@ -107,7 +106,7 @@ int32_t GuidanceInitialize()
 	queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 	
 	// Listen for updates.
-	AttitudeRawConnectQueue(queue);
+	AccelsConnectQueue(queue);
 	
 	return 0;
 }
@@ -157,11 +156,11 @@ static void guidanceTask(void *parameters)
 		}
 
 		// Collect downsampled attitude data
-		AttitudeRawData attitudeRaw;
+		AccelsData accels;
-		AttitudeRawGet(&attitudeRaw);		
+		AccelsGet(&accels);		
-		accel[0] += attitudeRaw.accels[0];
+		accel[0] += accels.x;
-		accel[1] += attitudeRaw.accels[1];
+		accel[1] += accels.y;
-		accel[2] += attitudeRaw.accels[2];
+		accel[2] += accels.z;
 		accel_accum++;
 
 		// Continue collecting data if not enough time
@@ -173,6 +172,8 @@ static void guidanceTask(void *parameters)
 		accel[0] /= accel_accum;
 		accel[1] /= accel_accum;
 		accel[2] /= accel_accum;
+		accel[0] = accel[1] = accel[2] = 0;
+		accel_accum = 0;
 		
 		//rotate avg accels into earth frame and store it
 		AttitudeActualData attitudeActual;
@@ -187,45 +188,30 @@ static void guidanceTask(void *parameters)
 			for (uint8_t j=0; j<3; j++)
 				accel_ned[i] += Rbe[j][i]*accel[j];
 		}
-		accel_ned[2] += 9.81;
+		accel_ned[2] += 9.81f;
 		
 		NedAccelData accelData;
 		NedAccelGet(&accelData);
-		// Convert from m/s to cm/s
 		accelData.North = accel_ned[0];
 		accelData.East = accel_ned[1];
 		accelData.Down = accel_ned[2];
 		NedAccelSet(&accelData);
 		
-		
 		FlightStatusGet(&flightStatus);
 		SystemSettingsGet(&systemSettings);
 		GuidanceSettingsGet(&guidanceSettings);
 		
-		if ((PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_GUIDANCE) &&
+		if ((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) &&
 		    ((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) ||
 		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) ||
 		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) ||
 		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ))
 		{
-			if(positionHoldLast == 0) {
-				/* When enter position hold mode save current position */
-				PositionDesiredData positionDesired;
-				PositionActualData positionActual;
-				PositionDesiredGet(&positionDesired);
-				PositionActualGet(&positionActual);
-				positionDesired.North = positionActual.North;
-				positionDesired.East = positionActual.East;
-				PositionDesiredSet(&positionDesired);
-				positionHoldLast = 1;
-			}
-			
 			if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ) 
 				updateVtolDesiredVelocity();
 			else 
 				manualSetDesiredVelocity();			
 			updateVtolDesiredAttitude();
-			
 		} else {
 			// Be cleaner and get rid of global variables
 			northVelIntegral = 0;
@@ -236,9 +222,6 @@ static void guidanceTask(void *parameters)
 			downPosIntegral = 0;
 			positionHoldLast = 0;
 		}
-		
-		accel[0] = accel[1] = accel[2] = 0;
-		accel_accum = 0;
 	}
 }
 

flight/Modules/ManualControl/manualcontrol.c

@@ -34,19 +34,20 @@
  */
 
 #include "openpilot.h"
-#include "manualcontrol.h"
+#include "accessorydesired.h"
-#include "manualcontrolsettings.h"
-#include "stabilizationsettings.h"
-#include "manualcontrolcommand.h"
 #include "actuatordesired.h"
-#include "stabilizationdesired.h"
+#include "altitudeholddesired.h"
+#include "baroaltitude.h"
 #include "flighttelemetrystats.h"
 #include "flightstatus.h"
-#include "accessorydesired.h"
+#include "manualcontrol.h"
-#include "receiveractivity.h"
+#include "manualcontrolsettings.h"
-#include "altitudeholddesired.h"
+#include "manualcontrolcommand.h"
 #include "positionactual.h"
-#include "baroaltitude.h"
+#include "positiondesired.h"
+#include "stabilizationsettings.h"
+#include "stabilizationdesired.h"
+#include "receiveractivity.h"
 
 // Private constants
 #if defined(PIOS_MANUAL_STACK_SIZE)
@@ -83,6 +84,7 @@ static portTickType lastSysTime;
 static void updateActuatorDesired(ManualControlCommandData * cmd);
 static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
 static void altitudeHoldDesired(ManualControlCommandData * cmd);
+static void positionDesired(ManualControlCommandData * cmd);
 static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
 static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
 static void setArmedIfChanged(uint8_t val);
@@ -383,6 +385,9 @@ static void manualControlTask(void *parameters)
 					case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
 						altitudeHoldDesired(&cmd);
 						break;
+					case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+						positionDesired(&cmd);
+						break;
 					default:
 						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
 				}
@@ -600,6 +605,41 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
 
 #if defined(REVOLUTION)
 // TODO: Need compile flag to exclude this from copter control
+/**
+ * @brief Update the position desired to current location when
+ * enabled and allow the waypoint to be moved by transmitter
+ */
+static void positionDesired(ManualControlCommandData * cmd)
+{
+	static portTickType lastSysTime;
+	portTickType thisSysTime;
+	float dT;
+
+	thisSysTime = xTaskGetTickCount();
+	dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
+	lastSysTime = thisSysTime;
+
+	if(dT > 1) {
+		// After not being in this mode for a while init at current height
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
+		
+		PositionDesiredData positionDesired;
+		PositionDesiredGet(&positionDesired);
+		positionDesired.North = positionActual.North;
+		positionDesired.East = positionActual.East;
+		positionDesired.Down = positionActual.Down;
+		PositionDesiredSet(&positionDesired);
+	} else {
+		// TODO: Implement moving magic waypoint with remote
+	}
+}
+
+/**
+ * @brief Update the altitude desired to current altitude when
+ * enabled and enable altitude mode for stabilization
+ * @todo: Need compile flag to exclude this from copter control
+ */
 static void altitudeHoldDesired(ManualControlCommandData * cmd)
 {
 	const float DEADBAND_HIGH = 0.55;

flight/Revolution/Makefile

@@ -49,11 +49,10 @@ endif
 FLASH_TOOL = OPENOCD
 
 # List of modules to include
-MODULES = Sensors Attitude/revolution ManualControl Stabilization Altitude/revolution Actuator GPS FirmwareIAP 
+MODULES = Sensors Attitude/revolution ManualControl Stabilization Altitude/revolution Actuator GPS FirmwareIAP AltitudeHold Guidance
-MODULES += AltitudeHold
 MODULES += CameraStab
+MODULES += OveroSync
 MODULES += Telemetry
-#MODULES += OveroSync
 PYMODULES = 
 #FlightPlan