rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Merged in mindnever/librepilot/LP-479_dshot (pull request #410)

Browse Source 
LP-479 Initial DShot implementation, F4 & F1

Approved-by: Alessio Morale <alessiomorale@gmail.com>
Approved-by: Mateusz Kaduk <mateusz.kaduk@gmail.com>
Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Philippe Renon <philippe_renon@yahoo.fr>
Approved-by: Vladimir Zidar <mr_w@mindnever.org>
This commit is contained in:
Vladimir Zidar 2017-04-30 14:02:28 +00:00 committed by Lalanne Laurent
commit 6b09e1a9df
14 changed files with 574 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
flight/modules/Actuator/actuator.c
View File

@ -960,6 +960,13 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value)
// Remap 1000-2000 range to 5-25µs
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], (value * ACTUATOR_MULTISHOT_PULSE_FACTOR) - 180);
break;
case ACTUATORSETTINGS_BANKMODE_DSHOT:
// Remap 0-2000 range to: 0 = disarmed, 1 to 47 = Reserved for special commands, 48 to 2047 = Active throttle control.
if (value > 0) {
value += 47; /* skip over reserved values */
}
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value);
break;
default:
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value);
break;
@ -990,9 +997,15 @@ static void actuator_update_rate_if_changed(bool force_update)
{
static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM];
static uint16_t prevDShotMode;
bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings.BankMode, sizeof(prevBankMode)) != 0);
bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0);
if (force_update || (prevDShotMode != actuatorSettings.DShotMode)) {
PIOS_Servo_DSHot_Rate(actuatorSettings.DShotMode);
prevDShotMode = actuatorSettings.DShotMode;
}
// check if any setting is changed
if (updateMode || updateFreq) {
/* Something has changed, apply the settings to HW */
@ -1000,30 +1013,36 @@ static void actuator_update_rate_if_changed(bool force_update)
uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 };
for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) {
PIOS_Servo_SetBankMode(i,
actuatorSettings.BankMode[i] ==
ACTUATORSETTINGS_BANKMODE_PWM ?
PIOS_SERVO_BANK_MODE_PWM :
PIOS_SERVO_BANK_MODE_SINGLE_PULSE
);
}
enum pios_servo_bank_mode servo_bank_mode = PIOS_SERVO_BANK_MODE_PWM;
switch (actuatorSettings.BankMode[i]) {
case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
case ACTUATORSETTINGS_BANKMODE_ONESHOT42:
case ACTUATORSETTINGS_BANKMODE_MULTISHOT:
freq[i] = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered
clock[i] = ACTUATOR_ONESHOT_CLOCK; // Setup an 12MHz timer clock
servo_bank_mode = PIOS_SERVO_BANK_MODE_SINGLE_PULSE;
break;
case ACTUATORSETTINGS_BANKMODE_PWMSYNC:
freq[i] = 100;
clock[i] = ACTUATOR_PWM_CLOCK;
servo_bank_mode = PIOS_SERVO_BANK_MODE_SINGLE_PULSE;
break;
case ACTUATORSETTINGS_BANKMODE_DSHOT:
freq[i] = 100;
clock[i] = ACTUATOR_PWM_CLOCK;
servo_bank_mode = PIOS_SERVO_BANK_MODE_DSHOT;
break;
default: // PWM
freq[i] = actuatorSettings.BankUpdateFreq[i];
clock[i] = ACTUATOR_PWM_CLOCK;
servo_bank_mode = PIOS_SERVO_BANK_MODE_PWM;
break;
}
if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) {
PIOS_Servo_SetBankMode(i, servo_bank_mode);
}
}
memcpy(prevBankMode,

442
flight/pios/common/pios_servo.c
View File

@ -28,6 +28,10 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/*
* DShot: Tribute belongs to dRonin, http://dRonin.org/ for sparking the idea of
* using gpio bitbang as more general solution over using timer dma.
*/
#include "pios.h"
@ -38,24 +42,48 @@
/* Private Function Prototypes */
#define PIOS_SERVO_GPIO_BANKS 3
static const struct pios_servo_cfg *servo_cfg;
// determine if the related timer will work in synchronous (or OneShot/OneShot125) One Pulse mode.
static uint8_t pios_servo_bank_mode[PIOS_SERVO_BANKS] = { 0 };
// used to skip updates when pulse length is higher than update cycle
static uint16_t pios_servo_bank_next_update[PIOS_SERVO_BANKS] = { 0 };
static uint16_t pios_servo_bank_max_pulse[PIOS_SERVO_BANKS] = { 0 };
// timer associated to each bank
static TIM_TypeDef *pios_servo_bank_timer[PIOS_SERVO_BANKS] = { 0 };
static volatile uint32_t *pios_servo_bsrr[PIOS_SERVO_GPIO_BANKS]; // GPIO banks
// index of bank used for each pin
static uint8_t *pios_servo_pin_bank;
struct pios_servo_bank {
enum pios_servo_bank_mode mode;
uint16_t next_update;
uint16_t max_pulse;
TIM_TypeDef *timer;
};
struct pios_servo_pin {
struct pios_servo_bank *bank;
uint8_t bank_nr;
uint8_t gpio_bank;
uint16_t value;
};
static struct pios_servo_bank pios_servo_banks[PIOS_SERVO_BANKS];
static struct pios_servo_pin *pios_servo_pins;
// Dshot timing
static uint32_t pios_dshot_t0h_raw;
static uint32_t pios_dshot_t1h_raw;
static uint32_t pios_dshot_t_raw;
static bool pios_servo_enabled = true;
#define PIOS_SERVO_TIMER_CLOCK 1000000
#define PIOS_SERVO_SAFE_MARGIN 50
#define DSHOT_TIMING_ADJUST 8
#define DSHOT_T0H_DIV 2666
#define DSHOT_T1H_DIV 1333
#define DSHOT_NUM_BITS 16
extern void PIOS_Servo_Disable()
{
if (!servo_cfg) {
@ -79,9 +107,96 @@ extern void PIOS_Servo_Disable()
#else
#error Unsupported MCU
#endif
GPIO_Init(chan->pin.gpio, &init);
GPIO_ResetBits(chan->pin.gpio, chan->pin.init.GPIO_Pin);
GPIO_Init(chan->pin.gpio, &init);
}
}
static void PIOS_Servo_SetupBank(uint8_t bank_nr)
{
struct pios_servo_bank *bank = &pios_servo_banks[bank_nr];
if (!bank->timer) {
return;
}
// Setup the timer accordingly
switch (bank->mode) {
case PIOS_SERVO_BANK_MODE_PWM:
case PIOS_SERVO_BANK_MODE_SINGLE_PULSE:
TIM_ARRPreloadConfig(bank->timer, ENABLE);
TIM_CtrlPWMOutputs(bank->timer, ENABLE);
TIM_SelectOnePulseMode(bank->timer, TIM_OPMode_Repetitive);
TIM_Cmd(bank->timer, ENABLE);
break;
default:;
// do not manage timers otherwise
}
// Setup GPIO/AF
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
if (chan->timer != bank->timer) { // Not interested in this bank
continue;
}
switch (bank->mode) {
case PIOS_SERVO_BANK_MODE_PWM:
case PIOS_SERVO_BANK_MODE_SINGLE_PULSE:
GPIO_Init(chan->pin.gpio, &chan->pin.init);
#if defined(STM32F40_41xxx) || defined(STM32F446xx) || defined(STM32F411xE)
GPIO_PinAFConfig(chan->pin.gpio, chan->pin.pin_source, chan->remap);
#elif defined(STM32F10X_MD)
if (chan->remap) {
GPIO_PinRemapConfig(chan->remap, ENABLE);
}
#else
#error Unsupported MCU
#endif
/* Set up for output compare function */
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_OC1Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_2:
TIM_OC2Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_3:
TIM_OC3Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_4:
TIM_OC4Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
}
break;
case PIOS_SERVO_BANK_MODE_DSHOT:
{
GPIO_InitTypeDef init = chan->pin.init;
#if defined(STM32F40_41xxx) || defined(STM32F446xx) || defined(STM32F411xE)
init.GPIO_Mode = GPIO_Mode_OUT;
#elif defined(STM32F10X_MD)
init.GPIO_Mode = GPIO_Mode_Out_PP;
#else
#error Unsupported MCU
#endif
GPIO_ResetBits(chan->pin.gpio, chan->pin.init.GPIO_Pin);
GPIO_Init(chan->pin.gpio, &init);
}
break;
default:;
}
}
}
@ -91,53 +206,28 @@ extern void PIOS_Servo_Enable()
return;
}
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
GPIO_Init(chan->pin.gpio, &chan->pin.init);
#if defined(STM32F40_41xxx) || defined(STM32F446xx) || defined(STM32F411xE)
GPIO_PinAFConfig(chan->pin.gpio, chan->pin.pin_source, chan->remap);
#elif defined(STM32F10X_MD)
if (chan->remap) {
GPIO_PinRemapConfig(chan->remap, ENABLE);
}
#else
#error Unsupported MCU
#endif
/* Set up for output compare function */
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_OC1Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_2:
TIM_OC2Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_3:
TIM_OC3Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_4:
TIM_OC4Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
}
}
for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) {
TIM_TypeDef *timer = pios_servo_bank_timer[i];
if (timer && (pios_servo_bank_mode[i] != PIOS_SERVO_BANK_MODE_NONE)) {
TIM_SelectOnePulseMode(timer, TIM_OPMode_Repetitive);
TIM_Cmd(timer, ENABLE);
}
PIOS_Servo_SetupBank(i);
}
pios_servo_enabled = true;
}
void PIOS_Servo_DSHot_Rate(uint32_t rate_in_khz)
{
if (rate_in_khz < 150) {
rate_in_khz = 150;
}
uint32_t raw_hz = PIOS_DELAY_GetRawHz();
uint32_t tmp = raw_hz / rate_in_khz;
pios_dshot_t0h_raw = (tmp / DSHOT_T0H_DIV) - DSHOT_TIMING_ADJUST;
pios_dshot_t1h_raw = (tmp / DSHOT_T1H_DIV) - DSHOT_TIMING_ADJUST;
pios_dshot_t_raw = (tmp / 1000) - DSHOT_TIMING_ADJUST;
}
/**
* Initialise Servos
*/
@ -145,9 +235,19 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
{
/* Store away the requested configuration */
servo_cfg = cfg;
pios_servo_pin_bank = pios_malloc(sizeof(uint8_t) * cfg->num_channels);
uint8_t bank = 0;
pios_servo_pins = pios_malloc(sizeof(*pios_servo_pins) * cfg->num_channels);
PIOS_Assert(pios_servo_pins);
memset(pios_servo_pins, 0, sizeof(*pios_servo_pins) * cfg->num_channels);
/* set default dshot timing */
PIOS_Servo_DSHot_Rate(300);
uint8_t timer_bank = 0;
uint8_t gpio_bank = 0;
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
bool new = true;
@ -157,20 +257,53 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
}
if (new) {
PIOS_Assert(bank < PIOS_SERVO_BANKS);
PIOS_Assert(timer_bank < PIOS_SERVO_BANKS);
struct pios_servo_bank *bank = &pios_servo_banks[timer_bank];
for (uint8_t j = i; j < servo_cfg->num_channels; j++) {
if (servo_cfg->channels[j].timer == chan->timer) {
pios_servo_pin_bank[j] = bank;
pios_servo_pins[j].bank = bank;
pios_servo_pins[j].bank_nr = timer_bank;
}
}
pios_servo_bank_timer[bank] = chan->timer;
TIM_ARRPreloadConfig(chan->timer, ENABLE);
TIM_CtrlPWMOutputs(chan->timer, ENABLE);
bank->timer = chan->timer;
bank->mode = PIOS_SERVO_BANK_MODE_NONE;
TIM_Cmd(chan->timer, DISABLE);
bank++;
timer_bank++;
}
// now map gpio banks
new = true;
for (uint8_t j = 0; (j < i) && new; j++) {
new &= chan->pin.gpio != servo_cfg->channels[j].pin.gpio;
}
if (new) {
PIOS_Assert(gpio_bank < PIOS_SERVO_GPIO_BANKS);
for (uint8_t j = i; j < servo_cfg->num_channels; j++) {
if (servo_cfg->channels[j].pin.gpio == chan->pin.gpio) {
pios_servo_pins[j].gpio_bank = gpio_bank;
}
}
#if defined(STM32F40_41xxx) || defined(STM32F446xx) || defined(STM32F411xE)
pios_servo_bsrr[gpio_bank] = (uint32_t *)&chan->pin.gpio->BSRRL;
#else
pios_servo_bsrr[gpio_bank] = &chan->pin.gpio->BSRR;
#endif
++gpio_bank;
}
}
static uint32_t dummy_bsrr;
for (int i = gpio_bank; i < PIOS_SERVO_GPIO_BANKS; ++i) {
pios_servo_bsrr[gpio_bank] = &dummy_bsrr;
}
PIOS_Servo_Enable();
@ -181,17 +314,121 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode)
{
PIOS_Assert(bank < PIOS_SERVO_BANKS);
pios_servo_bank_mode[bank] = mode;
pios_servo_banks[bank].mode = mode;
if (!pios_servo_enabled) {
return;
}
if (pios_servo_bank_timer[bank]) {
// Setup the timer accordingly
TIM_SelectOnePulseMode(pios_servo_bank_timer[bank], TIM_OPMode_Repetitive);
TIM_Cmd(pios_servo_bank_timer[bank], ENABLE);
PIOS_Servo_SetupBank(bank);
}
static void PIOS_Servo_DShot_Update()
{
uint32_t next;
uint32_t data[PIOS_SERVO_GPIO_BANKS];
uint16_t pins[PIOS_SERVO_GPIO_BANKS];
uint16_t buffer[DSHOT_NUM_BITS][PIOS_SERVO_GPIO_BANKS];
for (uint8_t i = 0; i < PIOS_SERVO_GPIO_BANKS; ++i) {
pins[i] = 0;
}
bool has_dshot = false;
memset(buffer, 0, sizeof(buffer));
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
struct pios_servo_pin *pin = &pios_servo_pins[i];
if (pin->bank->mode != PIOS_SERVO_BANK_MODE_DSHOT) {
continue;
}
has_dshot = true;
uint16_t payload = pin->value;
if (payload > 2047) {
payload = 2047;
}
payload <<= 5;
payload |= ((payload >> 4) & 0xf) ^
((payload >> 8) & 0xf) ^
((payload >> 12) & 0xf);
uint16_t gpio_pin = servo_cfg->channels[i].pin.init.GPIO_Pin;
for (int j = 0; j < DSHOT_NUM_BITS; ++j) {
if (!(payload & 0x8000)) {
buffer[j][pin->gpio_bank] |= gpio_pin;
}
payload <<= 1;
}
pins[pin->gpio_bank] |= gpio_pin;
pin->value = 0;
}
if (!has_dshot) {
return;
}
PIOS_IRQ_Disable();
uint32_t start = PIOS_DELAY_GetRaw();
for (int i = 0; i < DSHOT_NUM_BITS; ++i) {
// single bit:
COMPILER_BARRIER();
// 1. write 3x BSRR to set gpio high
for (int j = 0; j < PIOS_SERVO_GPIO_BANKS; ++j) {
*(pios_servo_bsrr[j]) = (uint32_t)pins[j];
}
// Prep data
for (int j = 0; j < PIOS_SERVO_GPIO_BANKS; ++j) {
data[j] = buffer[i][j] << 16;
}
// 2. wait until T0H, write 3x BSRR to clear whatever bits are set to 0
next = start + pios_dshot_t0h_raw;
while ((next - PIOS_DELAY_GetRaw()) < pios_dshot_t0h_raw) {
;
}
COMPILER_BARRIER();
for (int j = 0; j < PIOS_SERVO_GPIO_BANKS; ++j) {
*(pios_servo_bsrr[j]) = data[j];
}
// Prep data
for (int j = 0; j < PIOS_SERVO_GPIO_BANKS; ++j) {
data[j] = (uint32_t)pins[j] << 16;
}
// 3. wait until T1H, then write 3x BSRR to set all to low
next = start + pios_dshot_t1h_raw;
while ((next - PIOS_DELAY_GetRaw()) < pios_dshot_t1h_raw) {
;
}
COMPILER_BARRIER();
for (int j = 0; j < PIOS_SERVO_GPIO_BANKS; ++j) {
*(pios_servo_bsrr[j]) = data[j];
}
// 4. wait until Tend
start += pios_dshot_t_raw;
while ((start - PIOS_DELAY_GetRaw()) < pios_dshot_t_raw) {
;
}
}
PIOS_IRQ_Enable();
}
@ -202,20 +439,19 @@ void PIOS_Servo_Update()
}
for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) {
const TIM_TypeDef *timer = pios_servo_bank_timer[i];
if (timer && pios_servo_bank_mode[i] == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
struct pios_servo_bank *bank = &pios_servo_banks[i];
if (bank->timer && (bank->mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE)) {
// a pulse to be generated is longer than cycle period. skip this update.
if (TIM_GetCounter((TIM_TypeDef *)timer) > (uint32_t)(pios_servo_bank_next_update[i] + PIOS_SERVO_SAFE_MARGIN)) {
TIM_GenerateEvent((TIM_TypeDef *)timer, TIM_EventSource_Update);
pios_servo_bank_next_update[i] = pios_servo_bank_max_pulse[i];
if (TIM_GetCounter((TIM_TypeDef *)bank->timer) > (uint32_t)(bank->next_update + PIOS_SERVO_SAFE_MARGIN)) {
TIM_GenerateEvent((TIM_TypeDef *)bank->timer, TIM_EventSource_Update);
bank->next_update = bank->max_pulse;
}
}
pios_servo_bank_max_pulse[i] = 0;
bank->max_pulse = 0;
}
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
uint8_t bank = pios_servo_pin_bank[i];
uint8_t mode = pios_servo_bank_mode[bank];
if (mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
if (pios_servo_pins[i].bank->mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
/* Update the position */
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
@ -235,6 +471,8 @@ void PIOS_Servo_Update()
}
}
}
PIOS_Servo_DShot_Update();
}
/**
* Set the servo update rate (Max 500Hz)
@ -254,7 +492,7 @@ void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t ban
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
for (uint8_t i = 0; i < banks && i < PIOS_SERVO_BANKS; i++) {
const TIM_TypeDef *timer = pios_servo_bank_timer[i];
const TIM_TypeDef *timer = pios_servo_banks[i].timer;
if (timer) {
uint32_t new_clock = PIOS_SERVO_TIMER_CLOCK;
if (clock[i]) {
@ -298,37 +536,43 @@ void PIOS_Servo_Set(uint8_t servo, uint16_t position)
/* Update the position */
const struct pios_tim_channel *chan = &servo_cfg->channels[servo];
uint16_t val = position;
uint16_t margin = chan->timer->ARR / 50; // Leave 2% of period as margin to prevent overlaps
if (val > (chan->timer->ARR - margin)) {
val = chan->timer->ARR - margin;
}
uint8_t bank = pios_servo_pin_bank[servo];
if (pios_servo_bank_max_pulse[bank] < val) {
pios_servo_bank_max_pulse[bank] = val;
}
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_SetCompare1(chan->timer, val);
break;
case TIM_Channel_2:
TIM_SetCompare2(chan->timer, val);
break;
case TIM_Channel_3:
TIM_SetCompare3(chan->timer, val);
break;
case TIM_Channel_4:
TIM_SetCompare4(chan->timer, val);
break;
pios_servo_pins[servo].value = position;
const struct pios_tim_channel *chan = &servo_cfg->channels[servo];
struct pios_servo_bank *bank = pios_servo_pins[servo].bank;
if ((bank->mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) || (bank->mode == PIOS_SERVO_BANK_MODE_PWM)) {
uint16_t val = position;
uint16_t margin = chan->timer->ARR / 50; // Leave 2% of period as margin to prevent overlaps
if (val > (chan->timer->ARR - margin)) {
val = chan->timer->ARR - margin;
}
if (bank->max_pulse < val) {
bank->max_pulse = val;
}
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_SetCompare1(chan->timer, val);
break;
case TIM_Channel_2:
TIM_SetCompare2(chan->timer, val);
break;
case TIM_Channel_3:
TIM_SetCompare3(chan->timer, val);
break;
case TIM_Channel_4:
TIM_SetCompare4(chan->timer, val);
break;
}
}
}
uint8_t PIOS_Servo_GetPinBank(uint8_t pin)
{
if (pin < servo_cfg->num_channels) {
return pios_servo_pin_bank[pin];
return pios_servo_pins[pin].bank_nr;
} else {
return 0;
}

5
flight/pios/inc/pios_delay.h
View File

@ -32,13 +32,16 @@
#ifndef PIOS_DELAY_H
#define PIOS_DELAY_H
#ifndef USE_SIM_POSIX
#include "pios_delay_raw.h"
#endif
/* Public Functions */
extern int32_t PIOS_DELAY_Init(void);
extern int32_t PIOS_DELAY_WaituS(uint32_t uS);
extern int32_t PIOS_DELAY_WaitmS(uint32_t mS);
extern uint32_t PIOS_DELAY_GetuS();
extern uint32_t PIOS_DELAY_GetuSSince(uint32_t t);
extern uint32_t PIOS_DELAY_GetRaw();
extern uint32_t PIOS_DELAY_DiffuS(uint32_t raw);
extern uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later);

8
flight/pios/inc/pios_servo.h
View File

@ -33,15 +33,17 @@
/* Global types */
enum pios_servo_bank_mode {
PIOS_SERVO_BANK_MODE_NONE = 0,
PIOS_SERVO_BANK_MODE_PWM = 1,
PIOS_SERVO_BANK_MODE_SINGLE_PULSE = 2
PIOS_SERVO_BANK_MODE_NONE = 0,
PIOS_SERVO_BANK_MODE_PWM = 1,
PIOS_SERVO_BANK_MODE_SINGLE_PULSE = 2,
PIOS_SERVO_BANK_MODE_DSHOT = 3,
};
/* Public Functions */
extern void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks);
extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position);
extern void PIOS_Servo_Update();
extern void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode);
extern void PIOS_Servo_DSHot_Rate(uint32_t rate_in_khz);
extern uint8_t PIOS_Servo_GetPinBank(uint8_t pin);
/* ESC Bridge support */

36
flight/pios/stm32f0x/inc/pios_delay_raw.h Normal file
View File

@ -0,0 +1,36 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_DELAY Delay Functions
* @brief PiOS Delay functionality
* @{
*
* @file pios_delay_raw.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2017.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Settings functions header
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_DELAY_RAW_H
#define PIOS_DELAY_RAW_H
extern uint32_t PIOS_DELAY_GetRaw();
#endif /* PIOS_DELAY_RAW_H */

43
flight/pios/stm32f10x/inc/pios_delay_raw.h Normal file
View File

@ -0,0 +1,43 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_DELAY Delay Functions
* @brief PiOS Delay functionality
* @{
*
* @file pios_delay_raw.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2017.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Settings functions header
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_DELAY_RAW_H
#define PIOS_DELAY_RAW_H
/* these should be defined by CMSIS, but they aren't */
#define DWT_CTRL (*(volatile uint32_t *)0xe0001000)
#define CYCCNTENA (1 << 0)
#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004)
#define PIOS_DELAY_GetRaw() (DWT_CYCCNT)
extern uint32_t PIOS_DELAY_GetRawHz();
#endif /* PIOS_DELAY_RAW_H */

22
flight/pios/stm32f10x/pios_delay.c
View File

@ -34,14 +34,9 @@
#ifdef PIOS_INCLUDE_DELAY
/* these should be defined by CMSIS, but they aren't */
#define DWT_CTRL (*(volatile uint32_t *)0xe0001000)
#define CYCCNTENA (1 << 0)
#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004)
/* cycles per microsecond */
static uint32_t us_ticks;
static uint32_t raw_hz;
/**
* Initialises the Timer used by PIOS_DELAY functions.
@ -57,6 +52,7 @@ int32_t PIOS_DELAY_Init(void)
RCC_GetClocksFreq(&clocks);
us_ticks = clocks.SYSCLK_Frequency / 1000000;
PIOS_DEBUG_Assert(us_ticks > 1);
raw_hz = clocks.SYSCLK_Frequency;
/* turn on access to the DWT registers */
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
@ -84,7 +80,7 @@ int32_t PIOS_DELAY_WaituS(uint32_t uS)
uint32_t last_count = DWT_CYCCNT;
for (;;) {
uint32_t current_count = DWT_CYCCNT;
uint32_t current_count = PIOS_DELAY_GetRaw();
uint32_t elapsed_uS;
/* measure the time elapsed since the last time we checked */
@ -135,7 +131,7 @@ int32_t PIOS_DELAY_WaitmS(uint32_t mS)
*/
uint32_t PIOS_DELAY_GetuS(void)
{
return DWT_CYCCNT / us_ticks;
return PIOS_DELAY_GetRaw() / us_ticks;
}
/**
@ -149,12 +145,12 @@ uint32_t PIOS_DELAY_GetuSSince(uint32_t t)
}
/**
* @brief Get the raw delay timer, useful for timing
* @return Unitless value (uint32 wrap around)
* @brief Get the raw delay timer frequency
* @return raw delay timer frequency in Hz
*/
uint32_t PIOS_DELAY_GetRaw()
uint32_t PIOS_DELAY_GetRawHz()
{
return DWT_CYCCNT;
return raw_hz;
}
/**
@ -163,7 +159,7 @@ uint32_t PIOS_DELAY_GetRaw()
*/
uint32_t PIOS_DELAY_DiffuS(uint32_t raw)
{
uint32_t diff = DWT_CYCCNT - raw;
uint32_t diff = PIOS_DELAY_GetRaw() - raw;
return diff / us_ticks;
}

43
flight/pios/stm32f4xx/inc/pios_delay_raw.h Normal file
View File

@ -0,0 +1,43 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_DELAY Delay Functions
* @brief PiOS Delay functionality
* @{
*
* @file pios_delay_raw.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2017.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Settings functions header
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_DELAY_RAW_H
#define PIOS_DELAY_RAW_H
/* these should be defined by CMSIS, but they aren't */
#define DWT_CTRL (*(volatile uint32_t *)0xe0001000)
#define CYCCNTENA (1 << 0)
#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004)
#define PIOS_DELAY_GetRaw() (DWT_CYCCNT)
extern uint32_t PIOS_DELAY_GetRawHz();
#endif /* PIOS_DELAY_RAW_H */

24
flight/pios/stm32f4xx/pios_delay.c
View File

@ -35,14 +35,9 @@
#ifdef PIOS_INCLUDE_DELAY
/* these should be defined by CMSIS, but they aren't */
#define DWT_CTRL (*(volatile uint32_t *)0xe0001000)
#define CYCCNTENA (1 << 0)
#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004)
/* cycles per microsecond */
static uint32_t us_ticks;
static uint32_t raw_hz;
/**
* Initialises the Timer used by PIOS_DELAY functions.
@ -58,6 +53,7 @@ int32_t PIOS_DELAY_Init(void)
RCC_GetClocksFreq(&clocks);
us_ticks = clocks.SYSCLK_Frequency / 1000000;
PIOS_DEBUG_Assert(us_ticks > 1);
raw_hz = clocks.SYSCLK_Frequency;
/* turn on access to the DWT registers */
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
@ -82,10 +78,10 @@ int32_t PIOS_DELAY_Init(void)
int32_t PIOS_DELAY_WaituS(uint32_t uS)
{
uint32_t elapsed = 0;
uint32_t last_count = DWT_CYCCNT;
uint32_t last_count = PIOS_DELAY_GetRaw();
for (;;) {
uint32_t current_count = DWT_CYCCNT;
uint32_t current_count = PIOS_DELAY_GetRaw();
uint32_t elapsed_uS;
/* measure the time elapsed since the last time we checked */
@ -136,7 +132,7 @@ int32_t PIOS_DELAY_WaitmS(uint32_t mS)
*/
uint32_t PIOS_DELAY_GetuS()
{
return DWT_CYCCNT / us_ticks;
return PIOS_DELAY_GetRaw() / us_ticks;
}
/**
@ -150,12 +146,12 @@ uint32_t PIOS_DELAY_GetuSSince(uint32_t t)
}
/**
* @brief Get the raw delay timer, useful for timing
* @return Unitless value (uint32 wrap around)
* @brief Get the raw delay timer frequency
* @return raw delay timer frequency in Hz
*/
uint32_t PIOS_DELAY_GetRaw()
uint32_t PIOS_DELAY_GetRawHz()
{
return DWT_CYCCNT;
return raw_hz;
}
/**
@ -164,7 +160,7 @@ uint32_t PIOS_DELAY_GetRaw()
*/
uint32_t PIOS_DELAY_DiffuS(uint32_t raw)
{
uint32_t diff = DWT_CYCCNT - raw;
uint32_t diff = PIOS_DELAY_GetRaw() - raw;
return diff / us_ticks;
}

40
ground/gcs/src/plugins/config/configoutputwidget.cpp
View File

@ -51,6 +51,9 @@
#include <QTextEdit>
#include <QMessageBox>
#define MAXOUTPUT_VALUE 2500
#define MINOUTPUT_VALUE 500
ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(parent)
{
m_ui = new Ui_OutputWidget();
@ -396,11 +399,18 @@ void ConfigOutputWidget::refreshWidgetsValuesImpl(UAVObject *obj)
int minValue = actuatorSettingsData.ChannelMin[outputChannelForm->index()];
int maxValue = actuatorSettingsData.ChannelMax[outputChannelForm->index()];
outputChannelForm->setRange(minValue, maxValue);
if (channelBanks.count() > i) {
outputChannelForm->setBank(QString("%1").arg(channelBanks.at(i)));
outputChannelForm->setColor(m_banks.at(channelBanks.at(i++) - 1).color());
int bankNumber = channelBanks.at(i);
OutputBankControls bankControls = m_banks.at(bankNumber - 1);
setChannelLimits(outputChannelForm, &bankControls);
outputChannelForm->setBank(QString::number(bankNumber));
outputChannelForm->setColor(bankControls.color());
i++;
}
outputChannelForm->setRange(minValue, maxValue);
int neutral = actuatorSettingsData.ChannelNeutral[outputChannelForm->index()];
outputChannelForm->setNeutral(neutral);
}
@ -480,18 +490,42 @@ void ConfigOutputWidget::updateAlwaysStabilizeStatus()
}
}
void ConfigOutputWidget::setChannelLimits(OutputChannelForm *channelForm, OutputBankControls *bankControls)
{
switch (bankControls->modeCombo()->currentIndex()) {
case ActuatorSettings::BANKMODE_DSHOT:
channelForm->setLimits(0, 0, 0, 2000);
break;
// case ActuatorSettings::BANKMODE_BRUSHED:
// channelForm->setLimits(0, 0, 0, 100); // 0 to 100%
// break;
default:
channelForm->setLimits(MINOUTPUT_VALUE, MAXOUTPUT_VALUE, MINOUTPUT_VALUE, MAXOUTPUT_VALUE);
break;
}
}
void ConfigOutputWidget::onBankTypeChange()
{
QComboBox *bankModeCombo = qobject_cast<QComboBox *>(sender());
if (bankModeCombo != NULL) {
int bankNumber = 1;
QList<OutputChannelForm *> outputChannelForms = findChildren<OutputChannelForm *>();
foreach(OutputBankControls controls, m_banks) {
if (controls.modeCombo() == bankModeCombo) {
bool enabled = bankModeCombo->currentIndex() == ActuatorSettings::BANKMODE_PWM;
controls.rateCombo()->setEnabled(enabled);
controls.rateCombo()->setCurrentIndex(enabled ? 1 : 0);
foreach(OutputChannelForm * outputChannelForm, outputChannelForms) {
if (outputChannelForm->bank().toInt() == bankNumber) {
setChannelLimits(outputChannelForm, &controls);
}
}
break;
}
bankNumber++;
}
}
}

1
ground/gcs/src/plugins/config/configoutputwidget.h
View File

@ -104,6 +104,7 @@ private:
void assignOutputChannel(UAVDataObject *obj, QString &str);
void setColor(QWidget *widget, const QColor color);
void sendAllChannelTests();
void setChannelLimits(OutputChannelForm *channelForm, OutputBankControls *bankControls);
private slots:
void updateWarnings(UAVObject *);

25
ground/gcs/src/plugins/config/outputchannelform.cpp
View File

@ -29,9 +29,6 @@
#include "ui_outputchannelform.h"
#define MAXOUTPUT_VALUE 2500
#define MINOUTPUT_VALUE 500
OutputChannelForm::OutputChannelForm(const int index, QWidget *parent) :
ChannelForm(index, parent), ui(new Ui::outputChannelForm), m_inChannelTest(false)
{
@ -50,14 +47,6 @@ OutputChannelForm::OutputChannelForm(const int index, QWidget *parent) :
ui->actuatorLink->setChecked(false);
connect(ui->actuatorLink, SIGNAL(toggled(bool)), this, SLOT(linkToggled(bool)));
// Set limits
ui->actuatorMin->setMaximum(MAXOUTPUT_VALUE);
ui->actuatorMax->setMaximum(MAXOUTPUT_VALUE);
ui->actuatorValue->setMaximum(MAXOUTPUT_VALUE);
ui->actuatorMin->setMinimum(MINOUTPUT_VALUE);
ui->actuatorMax->setMinimum(MINOUTPUT_VALUE);
ui->actuatorValue->setMinimum(MINOUTPUT_VALUE);
setChannelRange();
disableMouseWheelEvents();
@ -144,7 +133,7 @@ void OutputChannelForm::linkToggled(bool state)
if (!parent()) {
return;
}
int min = MAXOUTPUT_VALUE;
int min = ui->actuatorMax->maximum();
int linked_count = 0;
QList<OutputChannelForm *> outputChannelForms = parent()->findChildren<OutputChannelForm *>();
// set the linked channels of the parent widget to the same value
@ -213,6 +202,18 @@ void OutputChannelForm::setNeutral(int value)
ui->actuatorNeutral->setValue(value);
}
/**
*
* Set
*/
void OutputChannelForm::setLimits(int actuatorMinMinimum, int actuatorMinMaximum, int actuatorMaxMinimum, int actuatorMaxMaximum)
{
ui->actuatorMin->setMaximum(actuatorMinMaximum);
ui->actuatorMax->setMaximum(actuatorMaxMaximum);
ui->actuatorMin->setMinimum(actuatorMinMinimum);
ui->actuatorMax->setMinimum(actuatorMaxMinimum);
}
/**
* Set minimal and maximal channel value.
*/

1
ground/gcs/src/plugins/config/outputchannelform.h
View File

@ -62,6 +62,7 @@ public slots:
void setRange(int minimum, int maximum);
void enableChannelTest(bool state);
QString outputMixerType();
void setLimits(int actuatorMinMinimum, int actuatorMinMaximum, int actuatorMaxMinimum, int actuatorMaxMaximum);
signals:
void channelChanged(int index, int value);

3
shared/uavobjectdefinition/actuatorsettings.xml
View File

@ -2,7 +2,8 @@
<object name="ActuatorSettings" singleinstance="true" settings="true" category="Control">
<description>Settings for the @ref ActuatorModule that controls the channel assignments for the mixer based on AircraftType</description>
<field name="BankUpdateFreq" units="Hz" type="uint16" elements="6" defaultvalue="50"/>
<field name="BankMode" type="enum" units="" elements="6" options="PWM,PWMSync,OneShot125,OneShot42,MultiShot" defaultvalue="PWM"/>
<field name="BankMode" type="enum" units="" elements="6" options="PWM,PWMSync,OneShot125,OneShot42,MultiShot,DShot" defaultvalue="PWM"/>
<field name="DShotMode" units="kHz" type="uint16" elements="1" defaultvalue="600" limits="%BE:150:1200"/>
<field name="ChannelMax" units="us" type="int16" elements="12" defaultvalue="1000"/>
<field name="ChannelNeutral" units="us" type="int16" elements="12" defaultvalue="1000"/>
<field name="ChannelMin" units="us" type="int16" elements="12" defaultvalue="1000"/>