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

AP_IOMCU: implement DSM bind

Browse Source
This commit is contained in:
Andrew Tridgell 2018-11-01 09:24:18 +11:00
parent 184c05bc70
commit f8dc17f8ff
2 changed files with 67 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -168,6 +168,10 @@ void AP_IOMCU_FW::init()
adc_init();
sbus_out_init();
// power on spektrum port
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL);
palSetLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN);
// we do no allocations after setup completes
reg_status.freemem = hal.util->available_memory();
}
@ -227,6 +231,9 @@ void AP_IOMCU_FW::update()
safety_update();
rcout_mode_update();
hal.rcout->timer_tick();
if (dsm_bind_state) {
dsm_bind_step();
}
}
}
@ -470,6 +477,7 @@ bool AP_IOMCU_FW::handle_code_write()
palSetLine(HAL_GPIO_PIN_SBUS_OUT_EN);
}
break;
case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
reg_setup.heater_duty_cycle = rx_io_packet.regs[0];
last_heater_ms = last_ms;
@ -492,6 +500,10 @@ bool AP_IOMCU_FW::handle_code_write()
reg_setup.ignore_safety = rx_io_packet.regs[0];
((ChibiOS::RCOutput *)hal.rcout)->set_safety_mask(reg_setup.ignore_safety);
break;
case PAGE_REG_SETUP_DSM_BIND:
dsm_bind_state = 1;
break;
default:
break;
@ -663,6 +675,58 @@ 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();

3
libraries/AP_IOMCU/iofirmware/iofirmware.h
View File

@ -35,6 +35,7 @@ private:
uint16_t mix_output_angle(uint8_t channel, int16_t angle) const;
uint16_t mix_output_range(uint8_t channel, int16_t value) const;
int16_t mix_elevon_vtail(int16_t angle1, int16_t angle2, bool first_output) const;
void dsm_bind_step(void);
struct PACKED {
/* default to RSSI ADC functionality */
@ -128,5 +129,7 @@ private:
uint32_t last_status_ms;
uint32_t last_ms;
uint32_t loop_counter;
uint8_t dsm_bind_state;
uint32_t last_dsm_bind_ms;
};