Merge branch 'altitudehold' into revolution_overo

Conflicts:
	flight/OpenPilot/UAVObjects.inc
	flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
	flight/Revolution/Makefile
	flight/Revolution/ins.c

flight/Modules/AltitudeHold/altitudehold.c

@@ -0,0 +1,180 @@
+/**
+ ******************************************************************************
+ *
+ * @file       guidance.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint 
+ * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
+ * of @ref ManualControlCommand is Auto.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Input object: ActiveWaypoint
+ * Input object: PositionActual
+ * Input object: ManualControlCommand
+ * Output object: AttitudeDesired
+ *
+ * This module will periodically update the value of the AttitudeDesired object.
+ *
+ * The module executes in its own thread in this example.
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+#include "openpilot.h"
+#include "altitudeholdsettings.h"
+#include "altitudeholddesired.h"	// object that will be updated by the module
+#include "positionactual.h"
+#include "stabilizationdesired.h"
+
+// Private constants
+#define MAX_QUEUE_SIZE 1
+#define STACK_SIZE_BYTES 1024
+#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
+// Private types
+
+// Private variables
+static xTaskHandle altitudeHoldTaskHandle;
+static xQueueHandle queue;
+
+// Private functions
+static void altitudeHoldTask(void *parameters);
+static void SettingsUpdatedCb(UAVObjEvent * ev);
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AltitudeHoldStart()
+{
+	// Start main task
+	xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle);
+//	TaskMonitorAdd(TASKINFO_RUNNING_GUIDANCE, guidanceTaskHandle);
+
+	return 0;
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AltitudeHoldInitialize()
+{
+	AltitudeHoldSettingsInitialize();
+	AltitudeHoldDesiredInitialize();
+
+	// Create object queue
+	queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
+	
+	// Listen for updates.
+	AltitudeHoldDesiredConnectQueue(queue);
+	
+	AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
+	
+	return 0;
+}
+MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart)
+
+static float throttleIntegral = 0;
+
+/**
+ * Module thread, should not return.
+ */
+static void altitudeHoldTask(void *parameters)
+{
+	AltitudeHoldSettingsData altitudeHoldSettings;
+	AltitudeHoldDesiredData altitudeHoldDesired;
+	PositionActualData positionActual;
+	StabilizationDesiredData stabilizationDesired;
+			
+	portTickType thisTime;
+	portTickType lastSysTime;
+	UAVObjEvent ev;
+	
+	// Force update of the settings
+	SettingsUpdatedCb(&ev);
+	
+	// Main task loop
+	lastSysTime = xTaskGetTickCount();
+	while (1) {
+
+		// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
+		if ( xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE )
+		{
+			// Todo: Add alarm if it should be running
+			throttleIntegral = 0;
+			continue;
+		} else {
+			PositionActualGet(&positionActual);
+			StabilizationDesiredGet(&stabilizationDesired);
+			float dT;
+			
+			thisTime = xTaskGetTickCount();
+			if(thisTime > lastSysTime) // reuse dt in case of wraparound
+				dT = (thisTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
+			lastSysTime = thisTime;
+
+			static float altitude;
+			const float altitudeTau = 0.1;
+			
+			
+			// Flipping sign on error since altitude is "down"
+			float error = - (altitudeHoldDesired.Down - positionActual.Down);
+			static float 
+			throttleIntegral += error * altitudeHoldSettings.Ki * dT * 1000;
+			if(throttleIntegral > altitudeHoldSettings.ILimit)
+				throttleIntegral = altitudeHoldSettings.ILimit;
+			else if (throttleIntegral < 0)
+				throttleIntegral = 0;
+			stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral;
+			if(stabilizationDesired.Throttle > 1)
+				stabilizationDesired.Throttle = 1;
+			else if (stabilizationDesired.Throttle < 0)
+				stabilizationDesired.Throttle = 0;
+				
+			stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+			stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+			stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;		
+			stabilizationDesired.Roll = altitudeHoldDesired.Roll;
+			stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
+			stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
+			StabilizationDesiredSet(&stabilizationDesired);
+		}
+	
+	}
+}
+
+static void SettingsUpdatedCb(UAVObjEvent * ev)
+{
+	AltitudeHoldDesiredGet(&altitudeHoldDesired);
+	AltitudeHoldSettingsGet(&altitudeHoldSettings);
+
+	const float fakeDt = 0.0025;
+	if(settings.GyroTau < 0.0001)
+		gyro_alpha = 0;   // not trusting this to resolve to 0
+	else
+		gyro_alpha = expf(-fakeDt  / settings.GyroTau);
+
+}

flight/Modules/AltitudeHold/inc/altitudehold.h

@@ -0,0 +1,31 @@
+/**
+ ******************************************************************************
+ *
+ * @file       examplemodperiodic.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Example module to be used as a template for actual modules.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef EXAMPLEMODPERIODIC_H
+#define EXAMPLEMODPERIODIC_H
+
+int32_t ExampleModPeriodicInitialize();
+int32_t GuidanceInitialize(void);
+#endif // EXAMPLEMODPERIODIC_H

flight/Modules/ManualControl/inc/manualcontrol.h

@@ -39,6 +39,7 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
 	(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \
+	(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
 	)

flight/Modules/ManualControl/manualcontrol.c

@@ -44,6 +44,8 @@
 #include "flightstatus.h"
 #include "accessorydesired.h"
 #include "receiveractivity.h"
+#include "altitudeholddesired.h"
+#include "positionactual.h"
 
 // Private constants
 #if defined(PIOS_MANUAL_STACK_SIZE)
@@ -79,6 +81,7 @@ static portTickType lastSysTime;
 // Private functions
 static void updateActuatorDesired(ManualControlCommandData * cmd);
 static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
+static void altitudeHoldDesired(ManualControlCommandData * cmd);
 static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
 static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
 static void setArmedIfChanged(uint8_t val);
@@ -375,7 +378,13 @@ static void manualControlTask(void *parameters)
 				updateStabilizationDesired(&cmd, &settings);
 				break;
 			case FLIGHTMODE_GUIDANCE:
-				// TODO: Implement
+				switch(flightStatus.FlightMode) {
+					case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
+						altitudeHoldDesired(&cmd);
+						break;
+					default:
+						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
+				}
 				break;
 		}
 	}
@@ -588,6 +597,47 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
 	StabilizationDesiredSet(&stabilization);
 }
 
+// TODO: Need compile flag to exclude this from copter control
+static void altitudeHoldDesired(ManualControlCommandData * cmd)
+{
+	const float DEADBAND_HIGH = 0.55;
+	const float DEADBAND_LOW = 0.45;
+	
+	static portTickType lastSysTime;
+	static bool zeroed = false;
+	portTickType thisSysTime;
+	float dT;
+	AltitudeHoldDesiredData altitudeHoldDesired;
+	AltitudeHoldDesiredGet(&altitudeHoldDesired);
+
+	StabilizationSettingsData stabSettings;
+	StabilizationSettingsGet(&stabSettings);
+
+	thisSysTime = xTaskGetTickCount();
+	if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
+		dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
+	lastSysTime = thisSysTime;
+
+	altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
+	altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
+	altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
+	
+	float currentDown;
+	PositionActualDownGet(&currentDown);
+	if(dT > 1) {
+		// After not being in this mode for a while init at current height
+		altitudeHoldDesired.Down = currentDown;
+		zeroed = false;
+	} else if (cmd->Throttle > DEADBAND_HIGH && zeroed)
+		altitudeHoldDesired.Down += (DEADBAND_HIGH - cmd->Throttle) * dT;
+	else if (cmd->Throttle < DEADBAND_LOW && zeroed)
+		altitudeHoldDesired.Down += (DEADBAND_LOW - cmd->Throttle) * dT;
+	else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH)  // Require the stick to enter the dead band before they can move height
+		zeroed = true;
+	
+	AltitudeHoldDesiredSet(&altitudeHoldDesired);
+}
+
 /**
  * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
  */

flight/Modules/Stabilization/stabilization.c

@@ -461,7 +461,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
 	if(settings.GyroTau < 0.0001)
 		gyro_alpha = 0;   // not trusting this to resolve to 0
 	else
-		gyro_alpha = exp(-fakeDt  / settings.GyroTau);
+		gyro_alpha = expf(-fakeDt  / settings.GyroTau);
 }
 
 

flight/Revolution/Makefile

@@ -49,7 +49,7 @@ 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
 MODULES += Telemetry
 MODULES += OveroSync
 PYMODULES = 

flight/Revolution/UAVObjects.inc

@@ -75,6 +75,8 @@ UAVOBJSRCFILENAMES += hwsettings
 UAVOBJSRCFILENAMES += receiveractivity
 UAVOBJSRCFILENAMES += cameradesired
 UAVOBJSRCFILENAMES += camerastabsettings
+UAVOBJSRCFILENAMES += altitudeholdsettings                                                                                                            
+UAVOBJSRCFILENAMES += altitudeholddesired
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

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

@@ -26,6 +26,9 @@ OTHER_FILES += UAVObjects.pluginspec
 HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/baroaltitude.h \
     $$UAVOBJECT_SYNTHETICS/attitudeactual.h \
+    $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
+    $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
+    $$UAVOBJECT_SYNTHETICS/inssettings.h \
     $$UAVOBJECT_SYNTHETICS/revocalibration.h \
     $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \
     $$UAVOBJECT_SYNTHETICS/gyros.h \
@@ -80,6 +83,9 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
 SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \
     $$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \
+    $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
+    $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/inssettings.cpp \
     $$UAVOBJECT_SYNTHETICS/revocalibration.cpp \
     $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \
     $$UAVOBJECT_SYNTHETICS/accels.cpp \

shared/uavobjectdefinition/altitudeholddesired.xml

@@ -0,0 +1,13 @@
+<xml>
+    <object name="AltitudeHoldDesired" singleinstance="true" settings="false">
+        <description>Holds the desired altitude (from manual control) as well as the desired attitude to pass through</description>
+        <field name="Down" units="m" type="float" elements="1"/>
+        <field name="Roll" units="deg" type="float" elements="1"/>
+        <field name="Pitch" units="deg" type="float" elements="1"/>
+	<field name="Yaw" units="deg/s" type="float" elements="1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="onchange" period="0"/>
+        <logging updatemode="periodic" period="1000"/>
+    </object>
+</xml>

shared/uavobjectdefinition/altitudeholdsettings.xml

@@ -0,0 +1,13 @@
+<xml>
+    <object name="AltitudeHoldSettings" singleinstance="true" settings="true">
+        <description>Settings for the @ref AltitudeHold module</description>
+	<field name="Kp" units="throttle/m" type="float" elements="1" defaultvalue="0.025"/>
+	<field name="Ki" units="throttle/m" type="float" elements="1" defaultvalue="0.025"/>
+	<field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.25"/>
+	<field name="AltitudeTau" units="s" type="float" elements="1" defaultvalue="0.1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+        <logging updatemode="never" period="0"/>
+    </object>
+</xml>

shared/uavobjectdefinition/flightstatus.xml

@@ -4,7 +4,7 @@
         <field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
 
 	<!-- Note these enumerated values should be the same as ManualControlSettings -->
-        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,VelocityControl,PositionHold"/>
+        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold"/>
 
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>

shared/uavobjectdefinition/manualcontrolsettings.xml

@@ -21,7 +21,7 @@
         <field name="Stabilization3Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling" defaultvalue="Attitude,Attitude,Rate"/>
 
 	<!-- Note these options values should be identical to those defined in FlightMode -->
-        <field name="FlightModePosition" units="" type="enum" elements="3" options="Manual,Stabilized1,Stabilized2,Stabilized3,VelocityControl,PositionHold" defaultvalue="Manual,Stabilized1,Stabilized2"/>
+        <field name="FlightModePosition" units="" type="enum" elements="3" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold" defaultvalue="Manual,Stabilized1,Stabilized2"/>
 
         <field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
         <access gcs="readwrite" flight="readwrite"/>