diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index 6ad511fdc..0944a356d 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -960,6 +960,12 @@ 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: + 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 +996,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 +1012,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, diff --git a/flight/pios/common/pios_servo.c b/flight/pios/common/pios_servo.c index 61592f5a7..41c703b87 100644 --- a/flight/pios/common/pios_servo.c +++ b/flight/pios/common/pios_servo.c @@ -38,24 +38,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 +103,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 +202,24 @@ 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) +{ + 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 +227,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 timinig */ + 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 +249,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 +306,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 +431,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 +463,8 @@ void PIOS_Servo_Update() } } } + + PIOS_Servo_DShot_Update(); } /** * Set the servo update rate (Max 500Hz) @@ -254,7 +484,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 +528,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; } diff --git a/flight/pios/inc/pios_delay.h b/flight/pios/inc/pios_delay.h index 63546d75d..45fbed7a8 100644 --- a/flight/pios/inc/pios_delay.h +++ b/flight/pios/inc/pios_delay.h @@ -32,13 +32,14 @@ #ifndef PIOS_DELAY_H #define PIOS_DELAY_H +#include "pios_delay_raw.h" + /* 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); diff --git a/flight/pios/inc/pios_servo.h b/flight/pios/inc/pios_servo.h index 2000eee25..bcc1fff22 100644 --- a/flight/pios/inc/pios_servo.h +++ b/flight/pios/inc/pios_servo.h @@ -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 */ diff --git a/flight/pios/stm32f0x/inc/pios_delay_raw.h b/flight/pios/stm32f0x/inc/pios_delay_raw.h new file mode 100644 index 000000000..769ea2f5e --- /dev/null +++ b/flight/pios/stm32f0x/inc/pios_delay_raw.h @@ -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 */ diff --git a/flight/pios/stm32f10x/inc/pios_delay_raw.h b/flight/pios/stm32f10x/inc/pios_delay_raw.h new file mode 100644 index 000000000..6a4941563 --- /dev/null +++ b/flight/pios/stm32f10x/inc/pios_delay_raw.h @@ -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 */ diff --git a/flight/pios/stm32f10x/pios_delay.c b/flight/pios/stm32f10x/pios_delay.c index 9effa1ec7..9907f2c17 100644 --- a/flight/pios/stm32f10x/pios_delay.c +++ b/flight/pios/stm32f10x/pios_delay.c @@ -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; } diff --git a/flight/pios/stm32f4xx/inc/pios_delay_raw.h b/flight/pios/stm32f4xx/inc/pios_delay_raw.h new file mode 100644 index 000000000..6a4941563 --- /dev/null +++ b/flight/pios/stm32f4xx/inc/pios_delay_raw.h @@ -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 */ diff --git a/flight/pios/stm32f4xx/pios_delay.c b/flight/pios/stm32f4xx/pios_delay.c index f9c5d62b1..272afc3f2 100644 --- a/flight/pios/stm32f4xx/pios_delay.c +++ b/flight/pios/stm32f4xx/pios_delay.c @@ -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; } diff --git a/ground/gcs/src/plugins/config/configoutputwidget.cpp b/ground/gcs/src/plugins/config/configoutputwidget.cpp index 0c7613f5b..301ff871a 100644 --- a/ground/gcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/gcs/src/plugins/config/configoutputwidget.cpp @@ -51,6 +51,9 @@ #include #include +#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("%1").arg(bankNumber)); + outputChannelForm->setColor(bankControls.color()); + + i++; } + outputChannelForm->setRange(minValue, maxValue); int neutral = actuatorSettingsData.ChannelNeutral[outputChannelForm->index()]; outputChannelForm->setNeutral(neutral); } @@ -480,18 +490,41 @@ 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); + } +} + void ConfigOutputWidget::onBankTypeChange() { QComboBox *bankModeCombo = qobject_cast(sender()); if (bankModeCombo != NULL) { + int bankNumber = 1; + QList outputChannelForms = findChildren(); 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++; } } } diff --git a/ground/gcs/src/plugins/config/configoutputwidget.h b/ground/gcs/src/plugins/config/configoutputwidget.h index bca11ddb0..b7e27e114 100644 --- a/ground/gcs/src/plugins/config/configoutputwidget.h +++ b/ground/gcs/src/plugins/config/configoutputwidget.h @@ -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 *); diff --git a/ground/gcs/src/plugins/config/outputchannelform.cpp b/ground/gcs/src/plugins/config/outputchannelform.cpp index b093956a1..973ff94c4 100644 --- a/ground/gcs/src/plugins/config/outputchannelform.cpp +++ b/ground/gcs/src/plugins/config/outputchannelform.cpp @@ -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->actuatorValue->maximum(); int linked_count = 0; QList outputChannelForms = parent()->findChildren(); // 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 min_low, int min_high, int max_low, int max_high) +{ + ui->actuatorMin->setMaximum(min_high); + ui->actuatorMax->setMaximum(max_high); + ui->actuatorMin->setMinimum(min_low); + ui->actuatorMax->setMinimum(max_low); +} + /** * Set minimal and maximal channel value. */ diff --git a/ground/gcs/src/plugins/config/outputchannelform.h b/ground/gcs/src/plugins/config/outputchannelform.h index 25cd5375a..ab63dabfa 100644 --- a/ground/gcs/src/plugins/config/outputchannelform.h +++ b/ground/gcs/src/plugins/config/outputchannelform.h @@ -62,6 +62,7 @@ public slots: void setRange(int minimum, int maximum); void enableChannelTest(bool state); QString outputMixerType(); + void setLimits(int min_low, int min_high, int max_low, int max_high); signals: void channelChanged(int index, int value); diff --git a/shared/uavobjectdefinition/actuatorsettings.xml b/shared/uavobjectdefinition/actuatorsettings.xml index 6aaed72f7..07fe0478a 100644 --- a/shared/uavobjectdefinition/actuatorsettings.xml +++ b/shared/uavobjectdefinition/actuatorsettings.xml @@ -2,7 +2,8 @@ Settings for the @ref ActuatorModule that controls the channel assignments for the mixer based on AircraftType - + +