Rework stm32l4_servo.c to use the EVENT_PERIOD to sequence throu the …

…timeout

libraries/Servo/src/Servo.cpp

@@ -79,7 +79,7 @@ uint8_t Servo::attach(int pin, int min, int max)
     pinMode(pin, OUTPUT);
 
     ServoTable.slot[this->servoIndex].pin = g_APinDescription[pin].pin;
-
+    
     this->min  = min;
     this->max  = max;
 
@@ -138,12 +138,12 @@ void Servo::writeMicroseconds(int width)
 	return;
     }
 
-    if (width < SERVO_PULSE_MIN) {
-	width = SERVO_PULSE_MIN;
+    if (width < this->min) {
+	width = this->min;
     }
 
-    if (width > SERVO_PULSE_MAX) {
-	width = SERVO_PULSE_MAX;
+    if (width > this->max) {
+	width = this->max;
     }
 
     ServoTable.slot[this->servoIndex].width = width;

libraries/Servo/src/Servo.h

@@ -61,13 +61,13 @@
 
 #define Servo_VERSION           2     // software version of this library
 
-#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo  
-#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo 
+#define MIN_PULSE_WIDTH      1000     // the shortest pulse sent to a servo  
+#define MAX_PULSE_WIDTH      2000     // the longest pulse sent to a servo 
 #define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
 #define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds 
 
-// NOTE: to maintain a strict refresh interval the user needs to not exceed 2000us pulse width
-#define MAX_SERVOS              9
+// NOTE: to maintain a strict refresh interval the user needs to not exceed 2250us pulse width
+#define MAX_SERVOS              8
 
 class Servo
 {

system/STM32L4xx/Include/stm32l4_servo.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016 Thomas Roell.  All rights reserved.
+ * Copyright (c) 2016-2017 Thomas Roell.  All rights reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to
@@ -40,7 +40,6 @@
 extern "C" {
 #endif
 
-#define SERVO_EVENT_UPDATE                       0x40000000
 #define SERVO_EVENT_SYNC                         0x80000000
 
 typedef void (*stm32l4_servo_callback_t)(void *context, uint32_t events);
@@ -51,14 +50,11 @@ typedef void (*stm32l4_servo_callback_t)(void *context, uint32_t events);
 #define SERVO_STATE_READY                        3
 #define SERVO_STATE_ACTIVE                       4
 
-#define SERVO_SLOT_COUNT                         9
+#define SERVO_SLOT_COUNT                         10
 
-#define SERVO_SYNC_MARGIN                        50     /* sync needs to be at least 50us after the last pulse      */
-#define SERVO_SYNC_WIDTH                         1950   /* last slot is 2ms, hence the default sync width is 1950us */
-#define SERVO_FRAME_WIDTH                        20000  /* the default RC servo frame is 10 slots or 20000us        */
-#define SERVO_PULSE_THRESHOLD                     500   /* below a pulse is deemed illegal                          */
-#define SERVO_PULSE_MIN                          1000
-#define SERVO_PULSE_MAX                          2000
+#define SERVO_SYNC_WIDTH                         100     /* sync needs to be at least 100us after the last pulse    */
+#define SERVO_FRAME_WIDTH                        20000   /* the default RC servo frame is 20000us                    */
+#define SERVO_PULSE_WIDTH                        100     /* below a pulse is deemed illegal                          */
 
 typedef struct _stm32l4_servo_table_t {
     uint32_t                    entries;
@@ -69,21 +65,19 @@ typedef struct _stm32l4_servo_table_t {
 } stm32l4_servo_table_t;
 
 typedef struct _stm32l4_servo_schedule_t {
-    uint16_t                    period;
-    uint16_t                    offset;
-    uint32_t                    entries;
+    uint16_t                    sync;
+    uint16_t                    entries;
     struct {
 	GPIO_TypeDef              *GPIO;
 	uint16_t                  mask;
-	uint16_t                  offset;
+	uint16_t                  width;
     }                           slot[SERVO_SLOT_COUNT];
 } stm32l4_servo_schedule_t;
 
 typedef struct _stm32l4_servo_t {
     volatile uint8_t                  state;
     uint8_t                           index;
     uint16_t                          prescaler;
-    uint16_t                          period;
     stm32l4_timer_t                   timer;
     stm32l4_servo_callback_t          callback;
     void                              *context;

system/STM32L4xx/Include/stm32l4_timer.h

@@ -145,6 +145,7 @@ extern bool     stm32l4_timer_notify(stm32l4_timer_t *timer, stm32l4_timer_callb
 extern bool     stm32l4_timer_start(stm32l4_timer_t *timer, bool oneshot);
 extern bool     stm32l4_timer_stop(stm32l4_timer_t *timer);
 extern uint32_t stm32l4_timer_count(stm32l4_timer_t *timer);
+extern bool     stm32l4_timer_period(stm32l4_timer_t *timer, uint32_t period, bool offset);
 extern bool     stm32l4_timer_channel(stm32l4_timer_t *timer, unsigned int channel, uint32_t compare, uint32_t control);
 extern bool     stm32l4_timer_compare(stm32l4_timer_t *timer, unsigned int channel, uint32_t compare);
 extern uint32_t stm32l4_timer_capture(stm32l4_timer_t *timer, unsigned int channel);

system/STM32L4xx/Source/stm32l4_servo.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016 Thomas Roell.  All rights reserved.
+ * Copyright (c) 2016-2017 Thomas Roell.  All rights reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to
@@ -42,96 +42,62 @@ static void stm32l4_servo_event_callback(void *context, uint32_t events)
     stm32l4_servo_schedule_t *active, *pending;
     unsigned int index;
 
-    if (events & TIMER_EVENT_PERIOD)
-    {
-	pending = servo->pending;
-	servo->pending = NULL;
-
-	if (pending)
-	{
-	    servo->active = pending;
-	}
-
-	active = servo->active;
+    index = servo->index;
+    active = servo->active;
 
-	if (active->entries != 0)
-	{
-	    if (servo->period == active->period)
-	    {
-		active->slot[0].GPIO->BSRR = active->slot[0].mask;
+    if (index != active->entries)
+    {
+	active->slot[index].GPIO->BRR = active->slot[index].mask;
 
-		stm32l4_timer_compare(&servo->timer, TIMER_CHANNEL_1, active->slot[0].offset);
-	    }
-	    else
-	    {
-		servo->period = active->period;
+	index++;
 
-		stm32l4_timer_stop(&servo->timer);
-		stm32l4_timer_configure(&servo->timer, servo->prescaler -1, servo->period -1, 0);
-		stm32l4_timer_channel(&servo->timer, TIMER_CHANNEL_1, active->slot[0].offset, TIMER_CONTROL_COMPARE_TIMING);
-		stm32l4_timer_start(&servo->timer, false);
-
-		active->slot[0].GPIO->BSRR = active->slot[0].mask;
+	if (index != active->entries)
+	{
+	    active->slot[index].GPIO->BSRR = active->slot[index].mask;
 
-		servo->state = SERVO_STATE_ACTIVE;
-	    }
+	    stm32l4_timer_period(&servo->timer, active->slot[index].width -1, true);
 	}
 	else
 	{
-	    stm32l4_timer_stop(&servo->timer);
-
-	    servo->active = NULL;
-
-	    servo->state = SERVO_STATE_READY;
+	    stm32l4_timer_period(&servo->timer, active->sync -1, true);
 	}
 
-	servo->index = 0;
-
-	if (pending && (servo->events & SERVO_EVENT_UPDATE))
-	{
-	    (*servo->callback)(servo->context, SERVO_EVENT_UPDATE);
-	}
+	servo->index = index;
     }
     else
     {
-	index = servo->index;
-	active = servo->active;
+	servo->index = 0;
 
-	if (index != active->entries)
+	pending = servo->pending;
+
+	if (pending == NULL)
 	{
-	    active->slot[index].GPIO->BRR = active->slot[index].mask;
-
-	    index++;
+	    active->slot[0].GPIO->BSRR = active->slot[0].mask;
 
-	    if (index != active->entries)
+	    stm32l4_timer_period(&servo->timer, active->slot[0].width -1, true);
+	}
+	else
+	{
+	    if (pending->entries)
 	    {
-		active->slot[index].GPIO->BSRR = active->slot[index].mask;
+		pending->slot[0].GPIO->BSRR = pending->slot[0].mask;
 
-		stm32l4_timer_compare(&servo->timer, TIMER_CHANNEL_1, active->slot[index].offset);
+		stm32l4_timer_period(&servo->timer, pending->slot[0].width -1, true);
 	    }
 	    else
 	    {
-		if (active->offset)
-		{
-		    stm32l4_timer_compare(&servo->timer, TIMER_CHANNEL_1, active->offset);
-		}
-		else
-		{
-		    if (servo->events & SERVO_EVENT_SYNC)
-		    {
-			(*servo->callback)(servo->context, SERVO_EVENT_SYNC);
-		    }
-		}
+		stm32l4_timer_stop(&servo->timer);
+
+		servo->state = SERVO_STATE_READY;
 	    }
 
-	    servo->index = index;
+	    servo->pending = NULL;
+	    servo->active = pending;
 	}
-	else
+
+	if (servo->events & SERVO_EVENT_SYNC)
 	{
-	    if (servo->events & SERVO_EVENT_SYNC)
-	    {
-		(*servo->callback)(servo->context, SERVO_EVENT_SYNC);
-	    }
+	    (*servo->callback)(servo->context, SERVO_EVENT_SYNC);
 	}
     }
 }
@@ -150,7 +116,6 @@ bool stm32l4_servo_create(stm32l4_servo_t *servo, unsigned int instance, unsigne
 
     servo->index = 0;
     servo->prescaler = 0;
-    servo->period = 0;
     servo->active = NULL;
     servo->pending = NULL;
 
@@ -178,7 +143,7 @@ bool stm32l4_servo_enable(stm32l4_servo_t *servo, const stm32l4_servo_table_t *t
 
     servo->state = SERVO_STATE_BUSY;
 
-    if (!stm32l4_timer_enable(&servo->timer, 0, 0, 0, stm32l4_servo_event_callback, servo, (TIMER_EVENT_PERIOD | TIMER_EVENT_CHANNEL_1)))
+    if (!stm32l4_timer_enable(&servo->timer, 0, 0, 0, stm32l4_servo_event_callback, servo, TIMER_EVENT_PERIOD))
     {
 	servo->state = SERVO_STATE_INIT;
 
@@ -233,22 +198,20 @@ bool stm32l4_servo_configure(stm32l4_servo_t *servo, const stm32l4_servo_table_t
 
     for (offset = 0, entry = 0, index = 0; entry < table->entries; entry++)
     {
-	if ((table->slot[entry].pin != GPIO_PIN_NONE) && (table->slot[index].width >= SERVO_PULSE_THRESHOLD))
+	if ((table->slot[entry].pin != GPIO_PIN_NONE) && (table->slot[index].width >= SERVO_PULSE_WIDTH))
 	{
-	    offset += table->slot[entry].width;
-
 	    pending->slot[index].GPIO = (GPIO_TypeDef *)(GPIOA_BASE + (GPIOB_BASE - GPIOA_BASE) * ((table->slot[entry].pin & GPIO_PIN_GROUP_MASK) >> GPIO_PIN_GROUP_SHIFT));
 	    pending->slot[index].mask = (1ul << ((table->slot[entry].pin & GPIO_PIN_INDEX_MASK) >> GPIO_PIN_INDEX_SHIFT));
-	    pending->slot[index].offset = offset;
+	    pending->slot[index].width = table->slot[entry].width;
 
+	    offset += table->slot[entry].width;
 	    index++;
 	}
     }
 
     if (offset == 0)
     {
-	pending->period = 0;
-	pending->offset = 0;
+	pending->sync = 0;
 	pending->entries = 0;
     }
     else
@@ -257,15 +220,13 @@ bool stm32l4_servo_configure(stm32l4_servo_t *servo, const stm32l4_servo_table_t
 
 	servo->prescaler = stm32l4_timer_clock(&servo->timer) / 1000000;
 
-	if ((offset + SERVO_SYNC_MARGIN + SERVO_SYNC_WIDTH) >= SERVO_FRAME_WIDTH)
+	if ((offset + SERVO_SYNC_WIDTH) >= SERVO_FRAME_WIDTH)
 	{
-	    pending->period = (offset + SERVO_SYNC_MARGIN + SERVO_SYNC_WIDTH);
-	    pending->offset = SERVO_SYNC_MARGIN;
+	    pending->sync = SERVO_SYNC_WIDTH;
 	}
 	else
 	{
-	    pending->period = SERVO_FRAME_WIDTH;
-	    pending->offset = SERVO_FRAME_WIDTH - SERVO_SYNC_WIDTH;
+	    pending->sync = SERVO_FRAME_WIDTH - offset;
 	}
     }
 
@@ -275,20 +236,17 @@ bool stm32l4_servo_configure(stm32l4_servo_t *servo, const stm32l4_servo_table_t
     }
     else
     {
-	if (pending->period)
+	if (pending->entries)
 	{
-	    servo->active = pending;
-
-	    servo->period = pending->period;
+	    servo->state = SERVO_STATE_ACTIVE;
 
-	    stm32l4_timer_stop(&servo->timer);
-	    stm32l4_timer_configure(&servo->timer, servo->prescaler -1, servo->period -1, 0);
-	    stm32l4_timer_channel(&servo->timer, TIMER_CHANNEL_1, pending->slot[0].offset, TIMER_CONTROL_COMPARE_TIMING);
+	    servo->active = pending;
+	    servo->index = 0;
+
+	    stm32l4_timer_configure(&servo->timer, servo->prescaler -1, pending->slot[0].width -1, 0);
 	    stm32l4_timer_start(&servo->timer, false);
 
 	    pending->slot[0].GPIO->BSRR = pending->slot[0].mask;
-
-	    servo->state = SERVO_STATE_ACTIVE;
 	}
     }
 

system/STM32L4xx/Source/stm32l4_timer.c

@@ -543,6 +543,34 @@ uint32_t stm32l4_timer_count(stm32l4_timer_t *timer)
     return TIM->CNT;
 }
 
+bool stm32l4_timer_period(stm32l4_timer_t *timer, uint32_t period, bool offset)
+{
+    TIM_TypeDef *TIM = timer->TIM;
+    uint32_t primask;
+
+    if ((timer->state != TIMER_STATE_READY) && (timer->state != TIMER_STATE_ACTIVE))
+    {
+	return false;
+    }
+
+    if (offset)
+    {
+	primask = __get_PRIMASK();
+
+	__disable_irq();
+
+	TIM->ARR = period - TIM->CNT;
+
+	__set_PRIMASK(primask);
+    }
+    else
+    {
+	TIM->ARR = period;
+    }
+
+    return true;
+}
+
 bool stm32l4_timer_channel(stm32l4_timer_t *timer, unsigned int channel, uint32_t compare, uint32_t control)
 {
     TIM_TypeDef *TIM = timer->TIM;