From 106bdca20e5003aa9e661d1874a863d8f52e8241 Mon Sep 17 00:00:00 2001
From: vassilis <vassilis>
Date: Mon, 14 Jun 2010 02:00:16 +0000
Subject: [PATCH] Flight/Actuator Go to failsafe actuator outputs when a
 command is not received

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@763 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 flight/OpenPilot/Makefile                    |  4 +-
 flight/OpenPilot/Modules/Actuator/actuator.c | 43 +++++++++++++++++++-
 2 files changed, 43 insertions(+), 4 deletions(-)

diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile
index 9a58c7783..c6b1688a3 100644
--- a/flight/OpenPilot/Makefile
+++ b/flight/OpenPilot/Makefile
@@ -45,9 +45,9 @@ FLASH_TOOL = OPENOCD
 USE_THUMB_MODE = YES
 
 # List of modules to include
-#MODULES = Telemetry GPS ManualControl Actuator Altitude Attitude
+MODULES = Telemetry GPS ManualControl Actuator Altitude Attitude
 #MODULES = Telemetry Example
-MODULES = Telemetry MK/MKSerial
+#MODULES = Telemetry MK/MKSerial
 
 
 # MCU name, submodel and board
diff --git a/flight/OpenPilot/Modules/Actuator/actuator.c b/flight/OpenPilot/Modules/Actuator/actuator.c
index dd3d96856..1e6ebc59d 100644
--- a/flight/OpenPilot/Modules/Actuator/actuator.c
+++ b/flight/OpenPilot/Modules/Actuator/actuator.c
@@ -35,6 +35,7 @@
 #define MAX_QUEUE_SIZE 2
 #define STACK_SIZE configMINIMAL_STACK_SIZE
 #define TASK_PRIORITY (tskIDLE_PRIORITY+4)
+#define FAILSAFE_TIMEOUT_MS 100
 
 // Private types
 
@@ -48,6 +49,7 @@ static int32_t mixerFixedWing(const ActuatorSettingsData* settings, const Actuat
 static int32_t mixerFixedWingElevon(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd);
 static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd);
 static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
+static void setFailsafe();
 
 /**
  * Module initialization
@@ -81,11 +83,18 @@ static void actuatorTask(void* parameters)
 	ActuatorSettingsGet(&settings);
 	PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]);
 
+	// Go to the neutral (failsafe) values until an ActuatorDesired update is received
+	setFailsafe();
+
 	// Main task loop
 	while (1)
 	{
-		// Wait until the ActuatorDesired object is updated
-		while ( xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE );
+		// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
+		if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
+		{
+			setFailsafe();
+			continue;
+		}
 
 		// Read settings
 		ActuatorSettingsGet(&settings);
@@ -236,3 +245,33 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
 	}
 	return valueScaled;
 }
+
+/**
+ * Set actuator output to the neutral values (failsafe)
+ */
+static void setFailsafe()
+{
+	ActuatorSettingsData settings;
+	ActuatorCommandData cmd;
+
+	// Read settings
+	ActuatorSettingsGet(&settings);
+
+	// Reset ActuatorCommand to neutral values
+	for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
+	{
+		cmd.Channel[n] = settings.ChannelNeutral[n];
+	}
+
+	// Set alarm
+	AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
+
+	// Update servo outputs
+	for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
+	{
+		PIOS_Servo_Set( n+1, cmd.Channel[n] );
+	}
+
+	// Update output object
+	ActuatorCommandSet(&cmd);
+}