diff --git a/libraries/Servo/src/Servo.cpp b/libraries/Servo/src/Servo.cpp index 4ad653cd..5ea8e60b 100644 --- a/libraries/Servo/src/Servo.cpp +++ b/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; diff --git a/libraries/Servo/src/Servo.h b/libraries/Servo/src/Servo.h index 6d1e172b..1f56fbd8 100644 --- a/libraries/Servo/src/Servo.h +++ b/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 { diff --git a/system/STM32L4xx/Include/stm32l4_servo.h b/system/STM32L4xx/Include/stm32l4_servo.h index ba4efa89..163f1d12 100644 --- a/system/STM32L4xx/Include/stm32l4_servo.h +++ b/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,13 +65,12 @@ 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; @@ -83,7 +78,6 @@ 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; diff --git a/system/STM32L4xx/Include/stm32l4_timer.h b/system/STM32L4xx/Include/stm32l4_timer.h index 7f04d807..3a831da3 100644 --- a/system/STM32L4xx/Include/stm32l4_timer.h +++ b/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); diff --git a/system/STM32L4xx/Lib/libstm32l432.a b/system/STM32L4xx/Lib/libstm32l432.a index 57ced6d3..4f58d0a9 100644 Binary files a/system/STM32L4xx/Lib/libstm32l432.a and b/system/STM32L4xx/Lib/libstm32l432.a differ diff --git a/system/STM32L4xx/Lib/libstm32l433.a b/system/STM32L4xx/Lib/libstm32l433.a index 53ffe2f2..3e8b1e93 100644 Binary files a/system/STM32L4xx/Lib/libstm32l433.a and b/system/STM32L4xx/Lib/libstm32l433.a differ diff --git a/system/STM32L4xx/Lib/libstm32l476.a b/system/STM32L4xx/Lib/libstm32l476.a index 5e11c58e..b2b6de22 100644 Binary files a/system/STM32L4xx/Lib/libstm32l476.a and b/system/STM32L4xx/Lib/libstm32l476.a differ diff --git a/system/STM32L4xx/Source/stm32l4_servo.c b/system/STM32L4xx/Source/stm32l4_servo.c index ddc0a8e0..487ebb85 100644 --- a/system/STM32L4xx/Source/stm32l4_servo.c +++ b/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; } } diff --git a/system/STM32L4xx/Source/stm32l4_timer.c b/system/STM32L4xx/Source/stm32l4_timer.c index 6187b04c..e3f82a86 100644 --- a/system/STM32L4xx/Source/stm32l4_timer.c +++ b/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;