Servo decoding and handling updates

main.c

@@ -90,6 +90,12 @@ static WORKING_AREA(sample_send_thread_wa, 1024);
 
 static Thread *sample_send_tp;
 
+#if USE_SERVO_INPUT
+static WORKING_AREA(servodec_handling_thread_wa, 1024);
+static Thread *servodec_handling_tp;
+static void servodec_func(void);
+#endif
+
 static msg_t periodic_thread(void *arg) {
 	(void)arg;
 
@@ -108,28 +114,6 @@ static msg_t periodic_thread(void *arg) {
 			ledpwm_set_intensity(LED_RED, 0.0);
 		}
 
-#if USE_SERVO_INPUT
-#define HYST		0.1
-#define CURR_FACT	40.0
-		// Use decoded servo inputs
-		if (servodec_get_time_since_update() < 500 && mcpwm_get_duty_cycle_now() > -0.01) {
-			float servo_val = (float)servodec_get_servo(0) / 128.0;
-			servo_val /= (1.0 - HYST);
-
-			if (servo_val > HYST) {
-				servo_val -= HYST;
-				mcpwm_set_current(servo_val * CURR_FACT);
-			} else if (servo_val < -HYST) {
-				servo_val += HYST;
-				mcpwm_set_current(servo_val * CURR_FACT);
-			} else {
-				mcpwm_set_duty(0.0);
-			}
-		} else {
-			mcpwm_set_duty(0.0);
-		}
-#endif
-
 		// Gurgalof bicycle-throttle
 #if USE_THROTTLE_ADC
 #define MIN_PWR	0.2
@@ -333,6 +317,47 @@ void main_process_packet(unsigned char *data, unsigned char len) {
 	}
 }
 
+#if USE_SERVO_INPUT
+static void servodec_func(void) {
+	chSysLockFromIsr();
+	chEvtSignalI(servodec_handling_tp, (eventmask_t) 1);
+	chSysUnlockFromIsr();
+}
+
+static msg_t servodec_handling_thread(void *arg) {
+	(void)arg;
+
+	chRegSetThreadName("Servodec handler");
+	servodec_handling_tp = chThdSelf();
+
+	for(;;) {
+		chEvtWaitAny((eventmask_t) 1);
+
+		// Use decoded servo inputs
+#define HYST		0.1
+#define CURR_FACT	40.0
+		if (servodec_get_time_since_update() < 500 && mcpwm_get_duty_cycle_now() > -0.01) {
+			float servo_val = servodec_get_servo_as_float(0);
+			servo_val /= (1.0 - HYST);
+
+			if (servo_val > HYST) {
+				servo_val -= HYST;
+				mcpwm_set_current(servo_val * CURR_FACT);
+			} else if (servo_val < -HYST) {
+				servo_val += HYST;
+				mcpwm_set_current(servo_val * CURR_FACT);
+			} else {
+				mcpwm_set_duty(0.0);
+			}
+		} else {
+			mcpwm_set_duty(0.0);
+		}
+	}
+
+	return 0;
+}
+#endif
+
 int main(void) {
 	halInit();
 	chSysInit();
@@ -345,12 +370,15 @@ int main(void) {
 #endif
 
 #if USE_SERVO_INPUT
-	servodec_init();
+	servodec_init(servodec_func);
 #endif
 
 	// Threads
 	chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL);
 	chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO, sample_send_thread, NULL);
+#if USE_SERVO_INPUT
+	chThdCreateStatic(servodec_handling_thread_wa, sizeof(servodec_handling_thread_wa), NORMALPRIO, servodec_handling_thread, NULL);
+#endif
 
 	for(;;) {
 		chThdSleepMilliseconds(100);

servo_dec.c

@@ -39,15 +39,28 @@
 #endif
 
 #define TIMER_FREQ				1000000
-#define INTERRUPT_TRESHOLD		4
+#define INTERRUPT_TRESHOLD		3
 
+// Private variables
 static volatile uint32_t interrupt_time = 0;
 static volatile int8_t servo_pos[SERVO_NUM];
 static volatile uint32_t time_since_update;
-static WORKING_AREA(timer_thread_wa, 128);
-static msg_t timer_thread(void *arg);
+static VirtualTimer vt;
 
-void servodec_init(void) {
+// Private functions
+static void update_counters(void *p);
+
+// Function pointers
+static void(*done_func)(void) = 0;
+
+/**
+ * Initialize the serve decoding driver.
+ *
+ * @param d_func
+ * A function that should be called every time the servo signals have been
+ * decoded. Can be NULL.
+ */
+void servodec_init(void (*d_func)(void)) {
 	// Initialize variables
 	time_since_update = 0;
 	interrupt_time = 0;
@@ -129,21 +142,24 @@ void servodec_init(void) {
 	/* TIM3 enable counter */
 	TIM_Cmd(TIM3, ENABLE);
 
-	chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
+	// Set up a virtual timer to update the counters
+	chSysLock();
+	chVTSetI(&vt, MS2ST(1), update_counters, NULL);
+	chSysUnlock();
+
+	// Set our function pointer
+	done_func = d_func;
 }
 
-static msg_t timer_thread(void *arg) {
-	(void)arg;
+static void update_counters(void *p) {
+	(void)p;
 
-	chRegSetThreadName("Servodec timer");
+	chSysLockFromIsr();
+	chVTSetI(&vt, MS2ST(1), update_counters, p);
+	chSysUnlockFromIsr();
 
-	for(;;) {
-		interrupt_time++;
-		time_since_update++;
-		chThdSleepMilliseconds(1);
-	}
-
-	return 0;
+	interrupt_time++;
+	time_since_update++;
 }
 
 void servodec_int_handler(void) {
@@ -189,11 +205,25 @@ void servodec_int_handler(void) {
 
 	if (curr_index == SERVO_NUM) {
 		time_since_update = 0;
+
+		// Call the function pointer if it is not NULL.
+		if (done_func) {
+			done_func();
+		}
 	}
 
 	interrupt_time = 0;
 }
 
+/**
+ * Get a decoded servo value as an integer.
+ *
+ * @param servo_num
+ * The servo index. If it is out of range, 0 will be returned.
+ *
+ * @return
+ * The servo value in the range [-128 127].
+ */
 int8_t servodec_get_servo(int servo_num) {
 	if (servo_num < SERVO_NUM) {
 		return servo_pos[servo_num];
@@ -202,9 +232,25 @@ int8_t servodec_get_servo(int servo_num) {
 	}
 }
 
-/*
+/**
+ * Get a decoded servo value as a float.
+ *
+ * @param servo_num
+ * The servo index. If it is out of range, 0 will be returned.
+ *
+ * @return
+ * The servo value in the range [-1.0 1.0].
+ */
+float servodec_get_servo_as_float(int servo_num) {
+	return (float)servodec_get_servo(servo_num) / 128.0;
+}
+
+/**
  * Get the amount of milliseconds that has passed since
- * the last time servo positions were received
+ * the last time servo positions were received.
+ *
+ * @return
+ * The amount of milliseconds that have passed since an update.
  */
 uint32_t servodec_get_time_since_update(void) {
 	return time_since_update;

servo_dec.h

@@ -33,10 +33,10 @@
 #define SERVODEC_IND_AUX			2
 
 // Functions
-void servodec_init(void);
-void servodec_timerfunc(void);
+void servodec_init(void (*d_func)(void));
 void servodec_int_handler(void);
 int8_t servodec_get_servo(int servo_num);
+float servodec_get_servo_as_float(int servo_num);
 uint32_t servodec_get_time_since_update(void);
 
 #endif /* SERVO_DEC_H_ */