aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_IOMCU: fixed delay in DSM bind

Browse Source
This commit is contained in:
Andrew Tridgell 2018-11-01 09:54:18 +11:00
parent 2488d25284
commit b9e4916c17
2 changed files with 75 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_IOMCU/iofirmware/iofirmware.cpp
View File

@ -505,7 +505,9 @@ bool AP_IOMCU_FW::handle_code_write()
break;
case PAGE_REG_SETUP_DSM_BIND:
dsm_bind_state = 1;
if (dsm_bind_state == 0) {
dsm_bind_state = 1;
}
break;
default:
@ -678,58 +680,6 @@ void AP_IOMCU_FW::fill_failsafe_pwm(void)
}
}
/*
perform a DSM bind operation
*/
void AP_IOMCU_FW::dsm_bind_step(void)
{
uint32_t now = last_ms;
switch (dsm_bind_state) {
case 1:
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL);
palClearLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN);
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_OUTPUT_PUSHPULL);
palSetLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
dsm_bind_state = 2;
last_dsm_bind_ms = now;
break;
case 2:
if (now - last_dsm_bind_ms >= 500) {
palSetLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN);
dsm_bind_state = 3;
last_dsm_bind_ms = now;
}
break;
case 3: {
if (now - last_dsm_bind_ms >= 72) {
// 9 pulses works with all satellite receivers, and supports the highest
// available protocol
const uint8_t num_pulses = 9;
for (uint8_t i=0; i<num_pulses; i++) {
hal.scheduler->delay_microseconds(120);
palClearLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
hal.scheduler->delay_microseconds(120);
palSetLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
}
last_dsm_bind_ms = now;
dsm_bind_state = 4;
}
break;
}
case 4:
if (now - last_dsm_bind_ms >= 50) {
// set back as alternate function with pullup for UART input
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
dsm_bind_state = 0;
}
break;
}
}
AP_HAL_MAIN();

72
libraries/AP_IOMCU/iofirmware/rc.cpp
View File

@ -19,9 +19,12 @@
#include "ch.h"
#include "hal.h"
#include "iofirmware.h"
#include "rc.h"
#include <AP_SBusOut/AP_SBusOut.h>
extern const AP_HAL::HAL& hal;
static const SerialConfig uart3_cfg = {
100000, // speed
USART_CR1_PCE | USART_CR1_M, // cr1, enable even parity
@ -42,3 +45,72 @@ void sbus_out_write(uint16_t *channels, uint8_t nchannels)
AP_SBusOut::sbus_format_frame(channels, nchannels, buffer);
chnWrite(&SD3, buffer, sizeof(buffer));
}
/*
sleep for 1ms using a busy loop
*/
static void delay_one_ms(uint32_t &now)
{
while (now == AP_HAL::millis()) ;
now = AP_HAL::millis();
}
/*
perform a DSM bind operation
*/
void AP_IOMCU_FW::dsm_bind_step(void)
{
uint32_t now = last_ms;
switch (dsm_bind_state) {
case 1:
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL);
palClearLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN);
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_OUTPUT_PUSHPULL);
palSetLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
dsm_bind_state = 2;
last_dsm_bind_ms = now;
break;
case 2:
if (now - last_dsm_bind_ms >= 500) {
palSetLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN);
dsm_bind_state = 3;
last_dsm_bind_ms = now;
}
break;
case 3: {
if (now - last_dsm_bind_ms >= 72) {
// 9 pulses works with all satellite receivers, and supports the highest
// available protocol
delay_one_ms(now);
const uint8_t num_pulses = 9;
for (uint8_t i=0; i<num_pulses; i++) {
// the delay should be 120us, but we are running our
// clock at 1kHz, and 1ms works fine
delay_one_ms(now);
palClearLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
delay_one_ms(now);
palSetLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
}
last_dsm_bind_ms = now;
dsm_bind_state = 4;
}
break;
}
case 4:
if (now - last_dsm_bind_ms >= 50) {
// set back as alternate function with pullup for UART input
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetLine(HAL_GPIO_PIN_SPEKTRUM_OUT);
dsm_bind_state = 0;
}
break;
default:
dsm_bind_state = 0;
break;
}
}