Browse Source

AP_RCProtocol: allow DSM bind using uart RX pin directly

gps-1.3.1
Andrew Tridgell 3 years ago
parent
commit
d8c0d82857
  1. 6
      libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
  2. 1
      libraries/AP_RCProtocol/AP_RCProtocol_DSM.h

6
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp

@ -353,6 +353,11 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_fra
*/ */
void AP_RCProtocol_DSM::start_bind(void) void AP_RCProtocol_DSM::start_bind(void)
{ {
#if defined(HAL_GPIO_SPEKTRUM_RC) && HAL_GPIO_SPEKTRUM_RC
if (!hal.gpio->get_mode(HAL_GPIO_SPEKTRUM_RC, bind_mode_saved)) {
return;
}
#endif
bind_state = BIND_STATE1; bind_state = BIND_STATE1;
} }
@ -408,6 +413,7 @@ void AP_RCProtocol_DSM::update(void)
if (now - bind_last_ms > 50) { if (now - bind_last_ms > 50) {
hal.gpio->pinMode(HAL_GPIO_SPEKTRUM_RC, 0); hal.gpio->pinMode(HAL_GPIO_SPEKTRUM_RC, 0);
bind_state = BIND_STATE_NONE; bind_state = BIND_STATE_NONE;
hal.gpio->set_mode(HAL_GPIO_SPEKTRUM_RC, bind_mode_saved);
} }
break; break;
} }

1
libraries/AP_RCProtocol/AP_RCProtocol_DSM.h

@ -57,6 +57,7 @@ private:
BIND_STATE4, BIND_STATE4,
} bind_state; } bind_state;
uint32_t bind_last_ms; uint32_t bind_last_ms;
uint32_t bind_mode_saved;
uint16_t last_values[AP_DSM_MAX_CHANNELS]; uint16_t last_values[AP_DSM_MAX_CHANNELS];

Loading…
Cancel
Save