From ab926da5072b0bcd61499efe9f68b82c5c5b26f4 Mon Sep 17 00:00:00 2001
From: Brian Webb <webbbn@gmail.com>
Date: Fri, 26 Apr 2013 02:16:11 +0100
Subject: [PATCH] OP-924: Added a supervisor timeout to the PPM output driver. 
 This is based on the code in the PPM input driver.  A (minor) uninitialize
 variable bug was also fixed in the PPM input driver.

---
 flight/pios/stm32f10x/pios_ppm.c     |  1 +
 flight/pios/stm32f10x/pios_ppm_out.c | 39 ++++++++++++++++++++++++++++
 2 files changed, 40 insertions(+)

diff --git a/flight/pios/stm32f10x/pios_ppm.c b/flight/pios/stm32f10x/pios_ppm.c
index 63e73fe39..f9d825162 100644
--- a/flight/pios/stm32f10x/pios_ppm.c
+++ b/flight/pios/stm32f10x/pios_ppm.c
@@ -188,6 +188,7 @@ extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg)
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
 	TIM_ICInitStructure.TIM_ICFilter = 0x0;
 
+	ppm_dev->supv_timer = 0;
 	if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) {
 		PIOS_DEBUG_Assert(0);
 	}
diff --git a/flight/pios/stm32f10x/pios_ppm_out.c b/flight/pios/stm32f10x/pios_ppm_out.c
index 6dd8c7915..1be7e465f 100644
--- a/flight/pios/stm32f10x/pios_ppm_out.c
+++ b/flight/pios/stm32f10x/pios_ppm_out.c
@@ -59,6 +59,8 @@ struct pios_ppm_out_dev {
 	bool Fresh;
 };
 
+static void PIOS_PPM_Out_Supervisor(uint32_t ppm_id);
+
 static bool PIOS_PPM_Out_validate(struct pios_ppm_out_dev *ppm_dev)
 {
 	return (ppm_dev->magic == PIOS_PPM_OUT_DEV_MAGIC);
@@ -188,6 +190,15 @@ int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *
 		TIM_SetCompare4(chan->timer, ppm_dev->triggering_period);
 		break;
 	}
+
+        // Configure the supervisor
+	ppm_dev->supv_timer = 0;
+	ppm_dev->Fresh = FALSE;
+	ppm_dev->Tracking = FALSE;
+	if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Out_Supervisor, (uint32_t)ppm_dev)) {
+		PIOS_DEBUG_Assert(0);
+	}
+
 	return 0;
 }
 
@@ -202,6 +213,10 @@ void PIOS_PPM_OUT_Set(uint32_t ppm_out_id, uint8_t servo, uint16_t position)
 		position = PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US;
 	if (position > PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US)
 		position = PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US;
+
+        // Update the supervisor tracking variables.
+	ppm_dev->Fresh = TRUE;
+	ppm_dev->Tracking = TRUE;
 	
 	// Update the position
 	ppm_dev->ChannelValue[servo] = position;
@@ -228,4 +243,28 @@ static void PIOS_PPM_OUT_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t
 	return;
 }
 
+static void PIOS_PPM_Out_Supervisor(uint32_t ppm_out_id) {
+	struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id;
+	if (!PIOS_PPM_Out_validate(ppm_dev))
+		return;
+
+	/* 
+	 * RTC runs at 625Hz so divide down the base rate so that this loop runs at 25Hz.
+	 */
+	if(++(ppm_dev->supv_timer) < 25) {
+		return;
+	}
+	ppm_dev->supv_timer = 0;
+
+	if (!ppm_dev->Fresh) {
+		ppm_dev->Tracking = FALSE;
+
+                // Flush counter variables
+                for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; i++)
+                        ppm_dev->ChannelValue[i] = 1000;
+	}
+
+	ppm_dev->Fresh = FALSE;
+}
+
 #endif /* PIOS_INCLUDE_PPM_OUT */