diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 477e021c64..2ad7763464 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -57,6 +57,13 @@ bool AP_RCProtocol::new_input() { bool ret = _new_input; _new_input = false; + + // run update function on backends + for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) { + if (backend[i] != nullptr) { + backend[i]->update(); + } + } return ret; } @@ -75,3 +82,15 @@ uint16_t AP_RCProtocol::read(uint8_t chan) } return 0; } + +/* + ask for bind start on supported receivers (eg spektrum satellite) + */ +void AP_RCProtocol::start_bind(void) +{ + for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) { + if (backend[i] != nullptr) { + backend[i]->start_bind(); + } + } +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index a400e74082..464c9cf49c 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -36,6 +36,8 @@ public: uint8_t num_channels(); uint16_t read(uint8_t chan); bool new_input(); + void start_bind(void); + private: enum rcprotocol_t _detected_protocol = NONE; AP_RCProtocol_Backend *backend[NONE]; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 0dbd33b59c..bc40c3b365 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -30,6 +30,12 @@ public: bool new_input(); uint8_t num_channels(); + // support for receivers that have FC initiated bind support + virtual void start_bind(void) {} + + // allow for backends that need regular polling + virtual void update(void) {} + protected: void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index fa8692e8f2..d4b54c2e3b 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -19,10 +19,10 @@ */ #include "AP_RCProtocol_DSM.h" -// #define DEBUG - -#ifdef DEBUG extern const AP_HAL::HAL& hal; + +// #define DEBUG +#ifdef DEBUG # define debug(fmt, args...) hal.console->printf(fmt "\n", ##args) #else # define debug(fmt, args...) do {} while(0) @@ -361,3 +361,71 @@ bool AP_RCProtocol_DSM::dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[ */ return true; } + + +/* + start bind on DSM satellites + */ +void AP_RCProtocol_DSM::start_bind(void) +{ + bind_state = BIND_STATE1; +} + + +/* + update function used for bind state machine + */ +void AP_RCProtocol_DSM::update(void) +{ +#if defined(HAL_GPIO_SPEKTRUM_PWR) && defined(HAL_GPIO_SPEKTRUM_RC) + switch (bind_state) { + case BIND_STATE_NONE: + break; + + case BIND_STATE1: + hal.gpio->write(HAL_GPIO_SPEKTRUM_PWR, !HAL_SPEKTRUM_PWR_ENABLED); + hal.gpio->pinMode(HAL_GPIO_SPEKTRUM_RC, 1); + hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1); + bind_last_ms = AP_HAL::millis(); + bind_state = BIND_STATE2; + break; + + case BIND_STATE2: { + uint32_t now = AP_HAL::millis(); + if (now - bind_last_ms > 500) { + hal.gpio->write(HAL_GPIO_SPEKTRUM_PWR, HAL_SPEKTRUM_PWR_ENABLED); + bind_last_ms = now; + bind_state = BIND_STATE3; + } + break; + } + + case BIND_STATE3: { + uint32_t now = AP_HAL::millis(); + if (now - bind_last_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; idelay_microseconds(120); + hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 0); + hal.scheduler->delay_microseconds(120); + hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1); + } + bind_last_ms = now; + bind_state = BIND_STATE4; + } + break; + } + + case BIND_STATE4: { + uint32_t now = AP_HAL::millis(); + if (now - bind_last_ms > 50) { + hal.gpio->pinMode(HAL_GPIO_SPEKTRUM_RC, 0); + bind_state = BIND_STATE_NONE; + } + break; + } + } +#endif +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h index 087be127df..0900750f18 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h @@ -23,6 +23,9 @@ class AP_RCProtocol_DSM : public AP_RCProtocol_Backend { public: AP_RCProtocol_DSM(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} void process_pulse(uint32_t width_s0, uint32_t width_s1) override; + void start_bind(void) override; + void update(void) override; + private: void dsm_decode(); bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); @@ -37,4 +40,15 @@ private: uint16_t bytes[16]; // including start bit and stop bit uint16_t bit_ofs; } dsm_state; -}; \ No newline at end of file + + // bind state machine + enum { + BIND_STATE_NONE, + BIND_STATE1, + BIND_STATE2, + BIND_STATE3, + BIND_STATE4, + } bind_state; + uint32_t bind_last_ms; + +};