From dcbbc86f34af8733818d902055419e334b444357 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2019 12:48:37 +1000 Subject: [PATCH] AP_Radio: added support for Skyviper 2018 model radios fixes #12273 --- libraries/AP_Radio/AP_Radio.cpp | 57 +- libraries/AP_Radio/AP_Radio.h | 28 +- libraries/AP_Radio/AP_Radio_backend.h | 71 +- libraries/AP_Radio/AP_Radio_bk2425.cpp | 1415 +++++++++++++++++++++++ libraries/AP_Radio/AP_Radio_bk2425.h | 261 +++++ libraries/AP_Radio/AP_Radio_cc2500.cpp | 935 +++++++++++---- libraries/AP_Radio/AP_Radio_cc2500.h | 57 +- libraries/AP_Radio/AP_Radio_cypress.cpp | 257 ++-- libraries/AP_Radio/AP_Radio_cypress.h | 45 +- libraries/AP_Radio/driver_bk2425.cpp | 698 +++++++++++ libraries/AP_Radio/driver_bk2425.h | 439 +++++++ libraries/AP_Radio/driver_cc2500.cpp | 13 +- libraries/AP_Radio/driver_cc2500.h | 26 +- libraries/AP_Radio/telem_structure.h | 52 +- 14 files changed, 3892 insertions(+), 462 deletions(-) create mode 100644 libraries/AP_Radio/AP_Radio_bk2425.cpp create mode 100644 libraries/AP_Radio/AP_Radio_bk2425.h create mode 100644 libraries/AP_Radio/driver_bk2425.cpp create mode 100644 libraries/AP_Radio/driver_bk2425.h diff --git a/libraries/AP_Radio/AP_Radio.cpp b/libraries/AP_Radio/AP_Radio.cpp index 642e351092..7ecbc993b7 100644 --- a/libraries/AP_Radio/AP_Radio.cpp +++ b/libraries/AP_Radio/AP_Radio.cpp @@ -6,6 +6,7 @@ #include "AP_Radio_backend.h" #include "AP_Radio_cypress.h" #include "AP_Radio_cc2500.h" +#include "AP_Radio_bk2425.h" extern const AP_HAL::HAL& hal; @@ -15,7 +16,8 @@ const AP_Param::GroupInfo AP_Radio::var_info[] = { // @Param: _TYPE // @DisplayName: Set type of direct attached radio // @Description: This enables support for direct attached radio receivers - // @Values: 0:None,1:CYRF6936 + // @Values: 0:None,1:CYRF6936,2:CC2500,3:BK2425 + // @User: Advanced AP_GROUPINFO("_TYPE", 1, AP_Radio, radio_type, 0), @@ -87,7 +89,7 @@ const AP_Param::GroupInfo AP_Radio::var_info[] = { // @Description: This sets the radio to a fixed test channel for factory testing. Using a fixed channel avoids the need for binding in factory testing. // @Values: 0:Disabled,1:TestChan1,2:TestChan2,3:TestChan3,4:TestChan4,5:TestChan5,6:TestChan6,7:TestChan7,8:TestChan8 // @User: Advanced - AP_GROUPINFO("_TESTCH", 11, AP_Radio, factory_test, 0), + AP_GROUPINFO("_TESTCH", 11, AP_Radio, factory_test, 0), // @Param: _TSIGCH // @DisplayName: RSSI value channel for telemetry data on transmitter @@ -108,7 +110,7 @@ const AP_Param::GroupInfo AP_Radio::var_info[] = { // @Description: Set transmitter maximum transmit power (from 1 to 8) // @Range: 1 8 // @User: Advanced - AP_GROUPINFO("_TXMAX", 14, AP_Radio, tx_max_power, 4), + AP_GROUPINFO("_TXMAX", 14, AP_Radio, tx_max_power, 8), // @Param: _BZOFS // @DisplayName: Transmitter buzzer adjustment @@ -130,7 +132,7 @@ const AP_Param::GroupInfo AP_Radio::var_info[] = { // @Range: 0 31 // @User: Advanced AP_GROUPINFO("_ABLVL", 17, AP_Radio, auto_bind_rssi, 0), - + AP_GROUPEND }; @@ -149,13 +151,39 @@ AP_Radio::AP_Radio(void) bool AP_Radio::init(void) { switch (radio_type) { +#if (not defined AP_RADIO_CYRF6936 || AP_RADIO_CYRF6936) case RADIO_TYPE_CYRF6936: driver = new AP_Radio_cypress(*this); break; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#if (not defined AP_RADIO_CC2500 || AP_RADIO_CC2500) case RADIO_TYPE_CC2500: driver = new AP_Radio_cc2500(*this); break; +#endif +#endif +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 +#if (not defined AP_RADIO_BK2425 || AP_RADIO_BK2425) + case RADIO_TYPE_BK2425: + driver = new AP_Radio_beken(*this); + break; +#endif +#endif +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 + case RADIO_TYPE_AUTO: + // auto-detect between cc2500 and beken radios +#if (not defined AP_RADIO_CC2500 || AP_RADIO_CC2500) + if (AP_Radio_cc2500::probe()) { + driver = new AP_Radio_cc2500(*this); + } +#endif +#if (not defined AP_RADIO_BK2425 || AP_RADIO_BK2425) + if (driver == nullptr) { + driver = new AP_Radio_beken(*this); + } +#endif + break; #endif default: break; @@ -227,6 +255,17 @@ void AP_Radio::handle_data_packet(mavlink_channel_t chan, const mavlink_data96_t } } +// play a tune on the TX +void AP_Radio::play_tune(const char *tune_str) +{ + mavlink_data96_t pkt {}; + uint8_t len = MIN(strlen(tune_str), 92); + pkt.len = len; + pkt.type = 43; + memcpy(&pkt.data[0], tune_str, len); + handle_data_packet(MAVLINK_COMM_0, pkt); +} + // update status, should be called from main thread void AP_Radio::update(void) { @@ -252,5 +291,15 @@ void AP_Radio::set_wifi_channel(uint8_t channel) } } +// change TX mode, toggling between mode1 and mode2 +void AP_Radio::change_txmode(void) +{ + if (stick_mode == 2) { + stick_mode.set_and_save_ifchanged(1); + } else { + stick_mode.set_and_save_ifchanged(2); + } +} + #endif // HAL_RCINPUT_WITH_AP_RADIO diff --git a/libraries/AP_Radio/AP_Radio.h b/libraries/AP_Radio/AP_Radio.h index 2f18f58d15..231f11504f 100644 --- a/libraries/AP_Radio/AP_Radio.h +++ b/libraries/AP_Radio/AP_Radio.h @@ -28,16 +28,16 @@ class AP_Radio { public: friend class AP_Radio_backend; - + // constructor AP_Radio(void); - + // init - initialise radio bool init(void); // reset the radio bool reset(void); - + // send a packet bool send(const uint8_t *pkt, uint16_t len); @@ -58,7 +58,7 @@ public: // get transmitter firmware version uint32_t get_tx_version(void); - + struct stats { uint32_t bad_packets; uint32_t recv_errors; @@ -71,22 +71,26 @@ public: RADIO_TYPE_NONE=0, RADIO_TYPE_CYRF6936=1, RADIO_TYPE_CC2500=2, + RADIO_TYPE_BK2425=3, + RADIO_TYPE_AUTO=100, }; - + enum ap_radio_protocol { PROTOCOL_AUTO=0, PROTOCOL_DSM2=1, PROTOCOL_DSMX=2, PROTOCOL_D16=3, + PROTOCOL_CC2500_GFSK=4, // deviation 57kHz for update of cc2500 with GFSK }; - + // get packet statistics const struct stats &get_stats(void); static const struct AP_Param::GroupInfo var_info[]; // get singleton instance - static AP_Radio *get_singleton(void) { + static AP_Radio *get_singleton(void) + { return _singleton; } @@ -95,7 +99,13 @@ public: // set the 2.4GHz wifi channel used by companion computer, so it can be avoided void set_wifi_channel(uint8_t channel); - + + // play a tune on the TX + void play_tune(const char *tune_str); + + // change TX mode + void change_txmode(void); + private: AP_Radio_backend *driver; @@ -116,6 +126,6 @@ private: AP_Int8 tx_buzzer_adjust; AP_Int8 auto_bind_time; AP_Int8 auto_bind_rssi; - + static AP_Radio *_singleton; }; diff --git a/libraries/AP_Radio/AP_Radio_backend.h b/libraries/AP_Radio/AP_Radio_backend.h index ea0d322f01..e6c3bda92b 100644 --- a/libraries/AP_Radio/AP_Radio_backend.h +++ b/libraries/AP_Radio/AP_Radio_backend.h @@ -26,13 +26,13 @@ class AP_Radio_backend public: AP_Radio_backend(AP_Radio &radio); virtual ~AP_Radio_backend(); - + // init - initialise radio virtual bool init(void) = 0; // init - reset radio virtual bool reset(void) = 0; - + // send a packet virtual bool send(const uint8_t *pkt, uint16_t len) = 0; @@ -56,82 +56,101 @@ public: // get TX fw version virtual uint32_t get_tx_version(void) = 0; - + // get radio statistics structure virtual const AP_Radio::stats &get_stats(void) = 0; // set the 2.4GHz wifi channel used by companion computer, so it can be avoided virtual void set_wifi_channel(uint8_t channel) = 0; - -protected: - AP_Radio::ap_radio_protocol get_protocol(void) const { +protected: + // These functions are for the radio code to access the parameters + // that the user can set using the web interface. + + AP_Radio::ap_radio_protocol get_protocol(void) const + { return (AP_Radio::ap_radio_protocol)radio.protocol.get(); } - uint8_t get_debug_level(void) const { + uint8_t get_debug_level(void) const + { return (uint8_t)radio.debug_level.get(); } - bool get_disable_crc(void) const { + bool get_disable_crc(void) const + { return (bool)radio.disable_crc.get(); } - - uint8_t get_rssi_chan(void) const { + + uint8_t get_rssi_chan(void) const + { return (uint8_t)radio.rssi_chan.get(); } - uint8_t get_pps_chan(void) const { + uint8_t get_pps_chan(void) const + { return (uint8_t)radio.pps_chan.get(); } - uint8_t get_tx_rssi_chan(void) const { + uint8_t get_tx_rssi_chan(void) const + { return (uint8_t)radio.tx_rssi_chan.get(); } - uint8_t get_tx_pps_chan(void) const { + uint8_t get_tx_pps_chan(void) const + { return (uint8_t)radio.tx_pps_chan.get(); } - - bool get_telem_enable(void) const { + + bool get_telem_enable(void) const + { return radio.telem_enable.get() > 0; } - uint8_t get_transmit_power(void) const { + uint8_t get_transmit_power(void) const + { return constrain_int16(radio.transmit_power.get(), 1, 8); } - uint8_t get_tx_max_power(void) const { + uint8_t get_tx_max_power(void) const + { return constrain_int16(radio.tx_max_power.get(), 1, 8); } - void set_tx_max_power_default(uint8_t v) { + void set_tx_max_power_default(uint8_t v) + { return radio.tx_max_power.set_default(v); } - - int8_t get_fcc_test(void) const { + + int8_t get_fcc_test(void) const + { return radio.fcc_test.get(); } - uint8_t get_stick_mode(void) const { + uint8_t get_stick_mode(void) const + { return radio.stick_mode.get(); } - uint8_t get_factory_test(void) const { + uint8_t get_factory_test(void) const + { return radio.factory_test.get(); } - uint8_t get_tx_buzzer_adjust(void) const { + uint8_t get_tx_buzzer_adjust(void) const + { return radio.tx_buzzer_adjust.get(); } - uint8_t get_autobind_time(void) const { + uint8_t get_autobind_time(void) const + { return radio.auto_bind_time.get(); } - uint8_t get_autobind_rssi(void) const { + uint8_t get_autobind_rssi(void) const + { return radio.auto_bind_rssi.get(); } - + AP_Radio &radio; }; diff --git a/libraries/AP_Radio/AP_Radio_bk2425.cpp b/libraries/AP_Radio/AP_Radio_bk2425.cpp new file mode 100644 index 0000000000..dbb9497e74 --- /dev/null +++ b/libraries/AP_Radio/AP_Radio_bk2425.cpp @@ -0,0 +1,1415 @@ +/* + driver for Beken_2425 radio + */ +#include + +//#pragma GCC optimize("O0") + +#if defined(HAL_RCINPUT_WITH_AP_RADIO) && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 + +#include +#include "AP_Radio_bk2425.h" +#include +#include +#include +#include +#include + +// start of 12 byte CPU ID +#ifndef UDID_START +#define UDID_START 0x1FFF7A10 +#endif + +#define TIMEOUT_PRIORITY 250 // Right above timer thread +#define EVT_TIMEOUT EVENT_MASK(0) // Event in the irq handler thread triggered by a timeout interrupt +#define EVT_IRQ EVENT_MASK(1) // Event in the irq handler thread triggered by a radio IRQ (Tx finished, Rx finished, MaxRetries limit) +#define EVT_BIND EVENT_MASK(2) // (not used yet) The user has clicked on the "start bind" button in the web interface (or equivalent). + +extern const AP_HAL::HAL& hal; + +// Output debug information on the UART, wrapped in MavLink packets +#define Debug(level, fmt, args...) do { if ((level) <= get_debug_level()) { hal.console->printf(fmt, ##args); }} while (0) +// Output fast debug information on the UART, in raw format. MavLink should be disabled if you want to understand these messages. +// This is for debugging issues with frequency hopping and synchronisation. +#define DebugPrintf(level, fmt, args...) do { if (AP_Radio_beken::radio_singleton && ((level) <= AP_Radio_beken::radio_singleton->get_debug_level())) { printf(fmt, ##args); }} while (0) +// Output debug information on the mavlink to the UART connected to the WiFi, wrapped in MavLink packets +#define DebugMavlink(level, fmt, args...) do { if ((level) <= get_debug_level()) { gcs().send_text(MAV_SEVERITY_INFO, fmt, ##args); }} while (0) + + +// object instance for trampoline +AP_Radio_beken *AP_Radio_beken::radio_singleton; +thread_t *AP_Radio_beken::_irq_handler_ctx; +virtual_timer_t AP_Radio_beken::timeout_vt; +// See variable definitions in AP_Radio_bk2425.h for comments +uint32_t AP_Radio_beken::isr_irq_time_us; +uint32_t AP_Radio_beken::isr_timeout_time_us; +uint32_t AP_Radio_beken::next_switch_us; +uint32_t AP_Radio_beken::bind_time_ms; +SyncTiming AP_Radio_beken::synctm; // Let the IRQ see the interpacket timing + +// ----------------------------------------------------------------------------- +// We have received a packet +// Sort out our timing relative to the tx to avoid clock drift +void SyncTiming::Rx(uint32_t when) +{ + uint32_t ld = delta_rx_time_us; + uint32_t d = when - rx_time_us; + if ((d > ld - DIFF_DELTA_RX) && (d < ld + DIFF_DELTA_RX)) { // Two deltas are similar to each other + if ((d > TARGET_DELTA_RX-SLOP_DELTA_RX) && (d < TARGET_DELTA_RX+SLOP_DELTA_RX)) { // delta is within range of single packet distance + // Use filter to change the estimate of the time in microseconds between the transmitters packet (according to OUR clock) + sync_time_us = ((sync_time_us * (256-16)) + (d * 16)) / 256; + } + } + rx_time_us = when; + delta_rx_time_us = d; + last_delta_rx_time_us = ld; +} + +// ----------------------------------------------------------------------------- +// Implement queuing (a 92 byte packet) in the circular buffer +void FwUpload::queue(const uint8_t *pSrc, uint8_t len) +{ + if (len == 0 || len > free_length()) { + return; // Safety check for out of space error + } + if (pending_head + len > SZ_BUFFER) { + uint8_t n = SZ_BUFFER-pending_head; + memcpy(&pending_data[pending_head], pSrc, n); + memcpy(&pending_data[0], pSrc+n, len-n); + } else { + memcpy(&pending_data[pending_head], pSrc, len); + } + pending_head = (pending_head + len) & (SZ_BUFFER-1); + added += len; +} + +// ----------------------------------------------------------------------------- +// Implement dequeing (a 16 byte packet) +void FwUpload::dequeue(uint8_t *pDst, uint8_t len) +{ + if (len == 0 || len > pending_length()) { + return; // Safety check for underflow error + } + if (pending_tail + len > SZ_BUFFER) { + uint8_t n = SZ_BUFFER-pending_tail; + memcpy(pDst, &pending_data[pending_tail], n); + memcpy(pDst+n, &pending_data[0], len-n); + } else { + memcpy(pDst, &pending_data[pending_tail], len); + } + pending_tail = (pending_tail + len) & (SZ_BUFFER-1); + sent += len; +} + + +// ----------------------------------------------------------------------------- + +/* + constructor + */ +AP_Radio_beken::AP_Radio_beken(AP_Radio &_radio) : + AP_Radio_backend(_radio), + beken(hal.spi->get_device("beken")) // trace this later - its on libraries/AP_HAL_ChibiOS/SPIDevice.cpp:92 +{ + // link to instance for irq_trampoline + + // (temporary) go into test mode + radio_singleton = this; + beken.fcc.fcc_mode = 0; + beken.fcc.channel = 23; + beken.fcc.power = 7+1; // Full power +} + +/* + initialise radio + */ +bool AP_Radio_beken::init(void) +{ + if (_irq_handler_ctx != nullptr) { + AP_HAL::panic("AP_Radio_beken: double instantiation of irq_handler\n"); + } + chVTObjectInit(&timeout_vt); + _irq_handler_ctx = chThdCreateFromHeap(NULL, + THD_WORKING_AREA_SIZE(2048), + "radio_bk2425", + TIMEOUT_PRIORITY, /* Initial priority. */ + irq_handler_thd, /* Thread function. */ + NULL); /* Thread parameter. */ + return reset(); +} + +/* + reset radio + */ +bool AP_Radio_beken::reset(void) +{ + if (!beken.lock_bus()) { + return false; + } + + radio_init(); + beken.unlock_bus(); + + return true; +} + +/* + return statistics structure from radio + */ +const AP_Radio::stats &AP_Radio_beken::get_stats(void) +{ + return stats; +} + +/* + read one pwm channel from radio + */ +uint16_t AP_Radio_beken::read(uint8_t chan) +{ + if (chan >= BEKEN_MAX_CHANNELS) { + return 0; + } + if (!valid_connection) { + return (chan < 4) ? 1500u : 0u; + } + return pwm_channels[chan]; +} + +/* + update status - called from main thread + */ +void AP_Radio_beken::update(void) +{ + check_fw_ack(); +} + + +/* + return number of active channels, and updates the data + */ +uint8_t AP_Radio_beken::num_channels(void) +{ + uint32_t now = AP_HAL::millis(); + uint8_t chan = get_rssi_chan(); + if ((chan > 0) && ((chan-1) < BEKEN_MAX_CHANNELS)) { + uint8_t value = BK_RSSI_DEFAULT; // Fixed value that will not update (halfway in the RSSI range for Cypress chips, 0..31) + if (beken.fcc.enable_cd) { + if (beken.fcc.last_cd) { + value += 4; + } else { + value -= 4; + } + } + if (t_status.pps == 0) { + value = BK_RSSI_MIN; // No packets = no RSSI + } + pwm_channels[chan-1] = value; + chan_count = MAX(chan_count, chan); + } + + chan = get_pps_chan(); + if ((chan > 0) && ((chan-1) < BEKEN_MAX_CHANNELS)) { + pwm_channels[chan-1] = t_status.pps; // How many packets received per second + chan_count = MAX(chan_count, chan); + } + + chan = get_tx_rssi_chan(); + if ((chan > 0) && ((chan-1) < BEKEN_MAX_CHANNELS)) { + pwm_channels[chan-1] = BK_RSSI_DEFAULT; // Fixed value that will not update (halfway in the RSSI range for Cypress chips, 0..31) + chan_count = MAX(chan_count, chan); + } + + chan = get_tx_pps_chan(); + if ((chan > 0) && ((chan-1) < BEKEN_MAX_CHANNELS)) { + pwm_channels[chan-1] = tx_pps; + chan_count = MAX(chan_count, chan); + } + + // Every second, update the statistics + if (now - last_pps_ms > 1000) { + last_pps_ms = now; + t_status.pps = stats.recv_packets - last_stats.recv_packets; + last_stats = stats; + if (stats.lost_packets != 0 || stats.timeouts != 0) { + Debug(3,"lost=%lu timeouts=%lu\n", stats.lost_packets, stats.timeouts); + } + stats.lost_packets=0; + stats.timeouts=0; + if (have_tx_pps == 1) { // Have we had tx pps recently? + tx_pps = 0; + } + if (have_tx_pps == 2) { // We have had it at some time + have_tx_pps = 1; // Not recently + } + } + return chan_count; +} + +/* + return time of last receive in microseconds + */ +uint32_t AP_Radio_beken::last_recv_us(void) +{ + return synctm.packet_timer; +} + +/* + send len bytes as a single packet + */ +bool AP_Radio_beken::send(const uint8_t *pkt, uint16_t len) +{ + // disabled for now + return false; +} + +// Borrow the CRC32 algorithm from AP_HAL_SITL +// Not exactly fast algorithm as it is bit based +#define CRC32_POLYNOMIAL 0xEDB88320L +static uint32_t CRC32Value(uint32_t icrc) +{ + int i; + uint32_t crc = icrc; + for ( i = 8 ; i > 0; i-- ) { + if ( crc & 1 ) { + crc = ( crc >> 1 ) ^ CRC32_POLYNOMIAL; + } else { + crc >>= 1; + } + } + return crc; +} + +static uint32_t CalculateBlockCRC32(uint32_t length, const uint8_t *buffer, uint32_t crc) +{ + while ( length-- != 0 ) { + crc = ((crc >> 8) & 0x00FFFFFFL) ^ (CRC32Value(((uint32_t) crc ^ *buffer++) & 0xff)); + } + return ( crc ); +} + +/* + initialise the radio + */ +void AP_Radio_beken::radio_init(void) +{ + DebugPrintf(1, "radio_init\r\n"); + beken.SetRBank(1); + uint8_t id = beken.ReadReg(BK2425_R1_WHOAMI); // id is now 99 + beken.SetRBank(0); // Reset to default register bank. + + if (id != BK_CHIP_ID_BK2425) { + + Debug(1, "bk2425: radio not found\n"); // We have to keep trying because it takes time to initialise + return; // Failure + } + + { + uint8_t serialid[12]; + memcpy(serialid, (const void *)UDID_START, 12); // 0x1FFF7A10ul on STM32F412 (see Util::get_system_id) + uint32_t drone_crc = CalculateBlockCRC32(12, serialid, 0xfffffffful); + if ((drone_crc & 0xff) == 0) { + ++drone_crc; // Ensure that the first byte (LSB) is non-zero for all drone CRC, for benefit of old (buggy) tx code. + } + myDroneId[0] = drone_crc; + myDroneId[1] = drone_crc >> 8; + myDroneId[2] = drone_crc >> 16; + myDroneId[3] = drone_crc >> 24; + DebugPrintf(1, "DroneCrc:%08x\r\n", drone_crc); + } + Debug(1, "beken: radio_init starting\n"); + + beken.bkReady = 0; + spd = beken.gTxSpeed; + beken.SwitchToIdleMode(); + hal.scheduler->delay(100); // delay more than 50ms. + + // Initialise Beken registers + beken.SetRBank(0); + beken.InitBank0Registers(beken.gTxSpeed); + beken.SetRBank(1); + beken.InitBank1Registers(beken.gTxSpeed); + hal.scheduler->delay(100); // delay more than 50ms. + beken.SetRBank(0); + + beken.SwitchToRxMode(); // switch to RX mode + beken.bkReady = 1; + hal.scheduler->delay_microseconds(10*1000); // 10ms seconds delay + + // setup handler for rising edge of IRQ pin + hal.gpio->attach_interrupt(HAL_GPIO_RADIO_IRQ, trigger_irq_radio_event, AP_HAL::GPIO::INTERRUPT_FALLING); + + if (load_bind_info()) { // See if we already have bound to the address of a tx + Debug(3,"Loaded bind info\n"); + nextChannel(1); + } + + beken.EnableCarrierDetect(true); // For autobinding + + isr_irq_time_us = isr_timeout_time_us = AP_HAL::micros(); + next_switch_us = isr_irq_time_us + 10000; + chVTSet(&timeout_vt, chTimeMS2I(10), trigger_timeout_event, nullptr); // Initial timeout? + if (3 <= get_debug_level()) { + beken.DumpRegisters(); + } +} + +// ---------------------------------------------------------------------------- +void AP_Radio_beken::trigger_irq_radio_event() +{ + //we are called from ISR context + // DEBUG2_HIGH(); + chSysLockFromISR(); + isr_irq_time_us = AP_HAL::micros(); + chEvtSignalI(_irq_handler_ctx, EVT_IRQ); + chSysUnlockFromISR(); + // DEBUG2_LOW(); +} + +// ---------------------------------------------------------------------------- +void AP_Radio_beken::trigger_timeout_event(void *arg) +{ + (void)arg; + //we are called from ISR context + // DEBUG2_HIGH(); + // DEBUG2_LOW(); + // DEBUG2_HIGH(); + isr_timeout_time_us = AP_HAL::micros(); + chSysLockFromISR(); + chEvtSignalI(_irq_handler_ctx, EVT_TIMEOUT); + chSysUnlockFromISR(); + // DEBUG2_LOW(); +} + +// ---------------------------------------------------------------------------- +// The user has clicked on the "Start Bind" button on the web interface +void AP_Radio_beken::start_recv_bind(void) +{ + chan_count = 0; + synctm.packet_timer = AP_HAL::micros(); + radio_singleton->bind_time_ms = AP_HAL::millis(); + chEvtSignal(_irq_handler_ctx, EVT_BIND); + Debug(1,"Starting bind\n"); +} + +// ---------------------------------------------------------------------------- +// handle a data96 mavlink packet for fw upload +void AP_Radio_beken::handle_data_packet(mavlink_channel_t chan, const mavlink_data96_t &m) +{ + if (sem.take_nonblocking()) { + fwupload.chan = chan; + fwupload.need_ack = false; + if (m.type == 43) { + // sending a tune to play - for development testing + Debug(4, "got tune data96 of len %u from chan %u\n", m.len, chan); + fwupload.reset(); + fwupload.fw_type = TELEM_PLAY; + fwupload.file_length = MIN(m.len, 90); + fwupload.file_length_round = (fwupload.file_length + 1 + 0x0f) & ~0x0f; // Round up to multiple of 16 (with nul-terminator) + fwupload.queue(&m.data[0], fwupload.file_length); + if (fwupload.file_length_round > fwupload.file_length) { + uint8_t pad[16] = {0}; + fwupload.queue(&pad[0], fwupload.file_length_round - fwupload.file_length); + } + } else { // m.type == 42 + // sending DFU + uint32_t ofs=0; + memcpy(&ofs, &m.data[0], 4); // Assumes the endianness of the data! + Debug(4, "got data96 of len %u from chan %u at offset %u\n", m.len, chan, unsigned(ofs)); + if (ofs == 0) { + fwupload.reset(); + // Typically file_length = 0x3906; file_length_round = 0x3980; + fwupload.file_length = ((uint16_t(m.data[4]) << 8) | (m.data[5])) + 6; // Add the header to the length + fwupload.file_length_round = (fwupload.file_length + 0x7f) & ~0x7f; // Round up to multiple of 128 + } + if (ofs != fwupload.added) { + fwupload.need_ack = true; // We want more data + } else { + // sending a chunk of firmware OTA upload + fwupload.fw_type = TELEM_FW; + fwupload.queue(&m.data[4], MIN(m.len-4, 92)); // This might fail if mavlink sends it too fast to me, in which case it will retry later + } + } + sem.give(); + } +} + + +// ---------------------------------------------------------------------------- +// Update the telemetry status variable; can be called in irq thread +// since the functions it calls are lightweight +void AP_Radio_beken::update_SRT_telemetry(void) +{ + t_status.flags = 0; + t_status.flags |= AP_Notify::flags.gps_status >= 3?TELEM_FLAG_GPS_OK:0; + t_status.flags |= AP_Notify::flags.pre_arm_check?TELEM_FLAG_ARM_OK:0; + t_status.flags |= AP_Notify::flags.failsafe_battery?0:TELEM_FLAG_BATT_OK; + t_status.flags |= hal.util->get_soft_armed()?TELEM_FLAG_ARMED:0; + t_status.flags |= AP_Notify::flags.have_pos_abs?TELEM_FLAG_POS_OK:0; + t_status.flags |= AP_Notify::flags.video_recording?TELEM_FLAG_VIDEO:0; + t_status.flight_mode = AP_Notify::flags.flight_mode; + t_status.tx_max = get_tx_max_power(); + t_status.note_adjust = get_tx_buzzer_adjust(); +} + +// ---------------------------------------------------------------------------- +// Update a radio control packet +// Called from IRQ context. +// Returns true for DFU or TUNE, false for telemetry +bool AP_Radio_beken::UpdateTxData(void) +{ + // send reboot command if appropriate + fwupload.counter++; + if ((fwupload.acked >= fwupload.file_length_round) && + (fwupload.fw_type == TELEM_FW) && // Not a tune request + (fwupload.rx_ack) && + (fwupload.acked >= 0x1000)) { // Sanity check + fwupload.rx_reboot = true; + } + if (fwupload.rx_reboot && // Sanity check + ((fwupload.counter & 0x01) != 0) && // Avoid starvation of telemetry + sem.take_nonblocking()) { // Is the other threads busy with fwupload data? + fwupload.rx_ack = false; + // Tell the Tx to reboot + packetFormatDfu* tx = &beken.pktDataDfu; + tx->packetType = BK_PKT_TYPE_DFU; + uint16_t addr = 0x0002; // Command to reboot + tx->address_lo = addr & 0xff; + tx->address_hi = (addr >> 8); + DebugPrintf(2, "reboot %u %u\r\n", fwupload.acked, fwupload.file_length_round); + sem.give(); + return true; + } else if ((fwupload.acked >= fwupload.file_length_round) && + (fwupload.fw_type == TELEM_PLAY) && // Atune request + (fwupload.rx_ack) && + ((fwupload.counter & 0x01) != 0) && // Avoid starvation of telemetry + (fwupload.acked > 0) && // Sanity check + sem.take_nonblocking()) { // Is the other threads busy with fwupload data? + fwupload.reset(); + // Tell the Tx the tune is complete + packetFormatDfu* tx = &beken.pktDataDfu; + tx->packetType = BK_PKT_TYPE_TUNE; + uint16_t addr = 0x0004; // Command to finalise the tune + tx->address_lo = addr & 0xff; + tx->address_hi = (addr >> 8); + sem.give(); + return true; + } + // send firmware update packet for 7/8 of packets if any data pending + else if ((fwupload.added >= (fwupload.acked + SZ_DFU)) && // Do we have a new packet to upload? + ((fwupload.counter & 0x07) != 0) && // Avoid starvation of telemetry + sem.take_nonblocking()) { // Is the other threads busy with fwupload data? + // Send DFU packet + packetFormatDfu* tx = &beken.pktDataDfu; + if (fwupload.sent > fwupload.acked) { + // Resend the last tx packet until it is acknowledged + DebugPrintf(4, "resend %u %u %u\r\n", fwupload.added, fwupload.sent, fwupload.acked); + } else if (fwupload.pending_length() >= SZ_DFU) { // safety check + // Send firmware update packet + uint16_t addr = fwupload.sent; + tx->address_lo = addr & 0xff; + tx->address_hi = (addr >> 8); + fwupload.dequeue(&tx->data[0], SZ_DFU); // (updated sent, pending_tail) + DebugPrintf(4, "send %u %u %u\r\n", fwupload.added, fwupload.sent, fwupload.acked); + if (fwupload.fw_type == TELEM_PLAY) { + tx->packetType = BK_PKT_TYPE_TUNE; + } else if (fwupload.fw_type == TELEM_FW) { + tx->packetType = BK_PKT_TYPE_DFU; + if (fwupload.free_length() > 96) { + fwupload.need_ack = true; // Request a new mavlink packet + } + } + } + sem.give(); + return true; + } else { + // Send telemetry packet + packetFormatTx* tx = &beken.pktDataTx; + update_SRT_telemetry(); + tx->packetType = BK_PKT_TYPE_TELEMETRY; ///< The packet type + tx->pps = t_status.pps; + tx->flags = t_status.flags; + tx->droneid[0] = myDroneId[0]; + tx->droneid[1] = myDroneId[1]; + tx->droneid[2] = myDroneId[2]; + tx->droneid[3] = myDroneId[3]; + tx->flight_mode = t_status.flight_mode; + tx->wifi = t_status.wifi_chan + (24 * t_status.tx_max); + tx->note_adjust = t_status.note_adjust; + // CPM bodge - use "Radio Protocol>0" to mean "Adaptive Frequency hopping disabled" + // This should move to a different parameter. + // Also the thresholds for swapping should move to be parameters. + if (get_protocol()) { + tx->hopping = 0; // Adaptive frequency hopping disabled + } else { + tx->hopping = adaptive.hopping; // Tell the tx what we want to use + } + telem_send_count++; + return false; + } + +} + +// ---------------------------------------------------------------------------- +// When (most of) a 92 byte packet has been sent to the Tx, ask for another one +// called from main thread +void AP_Radio_beken::check_fw_ack(void) +{ + if (fwupload.need_ack && sem.take_nonblocking()) { + // ack the send of a DATA96 fw packet to TX + if (fwupload.added < fwupload.file_length) { + fwupload.need_ack = false; + uint8_t data16[16] {}; + uint32_t ack_to = fwupload.added; + memcpy(&data16[0], &ack_to, 4); // Assume endianness matches + mavlink_msg_data16_send(fwupload.chan, 42, 4, data16); + } else if (fwupload.added & 0x7f) { // Are we on a boundary + // Pad out some bytes at the end + uint8_t data16[16]; + memset(&data16[0], 0, sizeof(data16)); + if (fwupload.free_length() > 16) { + fwupload.queue(&data16[0], 16-(fwupload.added & 15)); + } + DebugPrintf(4, "Pad to %d\r\n", fwupload.added); + } else if (fwupload.acked < fwupload.added) { + // Keep sending to the tx until it is acked + DebugPrintf(4, "PadResend %u %u %u\r\n", fwupload.added, fwupload.sent, fwupload.acked); + } else { + fwupload.need_ack = false; // All done + DebugPrintf(3, "StopUpload\r\n"); + uint8_t data16[16] {}; + uint32_t ack_to = fwupload.file_length; // Finished + memcpy(&data16[0], &ack_to, 4); // Assume endianness matches + mavlink_msg_data16_send(fwupload.chan, 42, 4, data16); + } + sem.give(); + } +} + +// ---------------------------------------------------------------------------- +/* support all 4 rc input modes by swapping channels. */ +void AP_Radio_beken::map_stick_mode(void) +{ + switch (get_stick_mode()) { + case 1: { + // mode1 = swap throttle and pitch + uint16_t tmp = pwm_channels[1]; + pwm_channels[1] = pwm_channels[2]; + pwm_channels[2] = tmp; + break; + } + + case 3: { + // mode3 = swap throttle and pitch, swap roll and yaw + uint16_t tmp = pwm_channels[1]; + pwm_channels[1] = pwm_channels[2]; + pwm_channels[2] = tmp; + tmp = pwm_channels[0]; + pwm_channels[0] = pwm_channels[3]; + pwm_channels[3] = tmp; + break; + } + + case 4: { + // mode4 = swap roll and yaw + uint16_t tmp = pwm_channels[0]; + pwm_channels[0] = pwm_channels[3]; + pwm_channels[3] = tmp; + break; + } + + case 2: + default: + // nothing to do, transmitter is natively mode2 + break; + } + + // reverse pitch input to match ArduPilot default + pwm_channels[1] = 3000 - pwm_channels[1]; +} + +// ---------------------------------------------------------------------------- +// This is a valid manual/auto binding packet. +// The type of binding is valid now, and it came with the right address. +// Lets check to see if it wants to be for another drone though +// Return 1 on double binding +uint8_t AP_Radio_beken::ProcessBindPacket(const packetFormatRx * rx) +{ + // Did the tx pick a drone yet? + uint32_t did = ((uint32_t)rx->u.bind.droneid[0]) | ((uint32_t)rx->u.bind.droneid[1] << 8) + | ((uint32_t)rx->u.bind.droneid[2] << 16) | ((uint32_t)rx->u.bind.droneid[3] << 24); + uint32_t mid = ((uint32_t)myDroneId[0]) | ((uint32_t)myDroneId[1] << 8) + | ((uint32_t)myDroneId[2] << 16) | ((uint32_t)myDroneId[3] << 24); + if (did & 0xff) { // If the first byte is zero, the drone id is not set (compatibility with old tx code) + // Is it me or someone else? + if (did != mid) { + // This tx is not for us! + if (!valid_connection && !already_bound) { + // Keep searching! + Debug(1, "WrongDroneId: %08lx vs %08lx\n", did, mid); + BadDroneId(); + return 1; + } + } + } + + // Set the address on which we are receiving the control data + syncch.SetChannel(rx->channel); // Can be factory test channels if wanted + if (get_factory_test() == 0) { // Final check that we are not in factory mode + adaptive.Invalidate(); + syncch.SetHopping(0, rx->u.bind.hopping); + beken.SetAddresses(&rx->u.bind.bind_address[0]); + Debug(1, " Bound to %x %x %x %x %x\r\n", rx->u.bind.bind_address[0], + rx->u.bind.bind_address[1], rx->u.bind.bind_address[2], + rx->u.bind.bind_address[3], rx->u.bind.bind_address[4]); + save_bind_info(); // May take some time + } + return 0; +} + + +// ---------------------------------------------------------------------------- +// Handle receiving a packet (we are still in an interrupt!) +// Return 1 if we want to stay on the current radio frequency instead of hopping (double binding) +uint8_t AP_Radio_beken::ProcessPacket(const uint8_t* packet, uint8_t rxaddr) +{ + uint8_t result = 0; + const packetFormatRx * rx = (const packetFormatRx *) packet; // Interpret the packet data + switch (rx->packetType) { + case BK_PKT_TYPE_CTRL_FOUND: + case BK_PKT_TYPE_CTRL_LOST: + // We haz data + if (rxaddr == 0) { + syncch.SetChannelIfSafe(rx->channel); + synctm.packet_timer = AP_HAL::micros(); // This is essential for letting the channels update + if (!already_bound) { + already_bound = true; // Do not autobind to a different tx unless we power off + // test rssi beken.EnableCarrierDetect(false); // Save 1ma of power + beken.WriteReg(BK_WRITE_REG|BK_EN_RXADDR, 0x01); // Ignore the binding channel, which might be from competing siren txs. + } + adaptive.Get(rx->channel); // Give the good news to the adaptive logic + // Put the data into the control values (assuming mode2) + pwm_channels[0] = 1000 + rx->u.ctrl.roll + (uint16_t(rx->u.ctrl.msb & 0xC0) << 2); // Roll + pwm_channels[1] = 1000 + rx->u.ctrl.pitch + (uint16_t(rx->u.ctrl.msb & 0x30) << 4); // Pitch + pwm_channels[2] = 1000 + rx->u.ctrl.throttle + (uint16_t(rx->u.ctrl.msb & 0x0C) << 6); // Throttle + pwm_channels[3] = 1000 + rx->u.ctrl.yaw + (uint16_t(rx->u.ctrl.msb & 0x03) << 8); // Yaw + pwm_channels[4] = 1000 + ((rx->u.ctrl.buttons_held & 0x07) >> 0) * 100; // SW1, SW2, SW3 + pwm_channels[5] = 1000 + ((rx->u.ctrl.buttons_held & 0x38) >> 3) * 100; // SW4, SW5, SW6 + // cope with mode1/mode2/mode3/mode4 + map_stick_mode(); + chan_count = MAX(chan_count, 7); + switch (rx->u.ctrl.data_type) { + case BK_INFO_FW_VER: break; + case BK_INFO_DFU_RX: { + uint16_t ofs = rx->u.ctrl.data_value_hi; + ofs <<= 8; + ofs |= rx->u.ctrl.data_value_lo; + if (ofs == fwupload.acked + SZ_DFU) { + fwupload.acked = ofs; + } + if ((ofs == fwupload.acked) && (ofs > 0)) { + fwupload.rx_ack = true; + } + if ((ofs == 0) && fwupload.rx_reboot) { + fwupload.reset(); + } + } + break; + case BK_INFO_FW_CRC_LO: + break; + case BK_INFO_FW_CRC_HI: + break; + case BK_INFO_FW_YM: + tx_date.firmware_year = rx->u.ctrl.data_value_hi; + tx_date.firmware_month = rx->u.ctrl.data_value_lo; + break; + case BK_INFO_FW_DAY: + tx_date.firmware_day = rx->u.ctrl.data_value_hi; + break; + case BK_INFO_MODEL: + break; + case BK_INFO_PPS: + tx_pps = rx->u.ctrl.data_value_lo; // Remember pps from tx + if (!have_tx_pps) { + have_tx_pps = 2; + if (tx_pps == 0) { // Has the tx not been receiving telemetry from someone else recently? + valid_connection = true; + } else { + // the TX has received more telemetry packets in the last second + // than we have ever sent. There must be another RX sending + // telemetry packets. We will reset our mfg_id and go back waiting + // for a new bind packet, hopefully with the right TX + Debug(1, "Double-bind detected via PPS %d\n", (int) tx_pps); + BadDroneId(); + result = 1; + } + } else { + have_tx_pps = 2; + } + break; + case BK_INFO_BATTERY: + // "voltage from TX is in 0.025 volt units". Convert to 0.01 volt units for easier display + // The CC2500 code (and this) actually assumes it is in 0.04 volt units, hence the tx scaling by 23/156 (38/256) instead of 60/256) + // Which means a maximum value is 152 units representing 6.0v rather than 240 units representing 6.0v + pwm_channels[6] = rx->u.ctrl.data_value_lo * 4; + break; + case BK_INFO_COUNTDOWN: + if (get_factory_test() == 0) { + if (rx->u.ctrl.data_value_lo) { + syncch.SetCountdown(rx->u.ctrl.data_value_lo+1, rx->u.ctrl.data_value_hi); + adaptive.Invalidate(); + DebugPrintf(2, "(%d) ", rx->u.ctrl.data_value_lo); + } + } + break; + case BK_INFO_HOPPING0: + if (get_factory_test() == 0) { + syncch.SetHopping(rx->u.ctrl.data_value_lo, rx->u.ctrl.data_value_hi); + // DebugPrintf(2, "[%d] ", rx->u.ctrl.data_value_lo); + } + break; + case BK_INFO_HOPPING1: // Ignored so far + break; + case BK_INFO_DRONEID0: // Does this Tx even want to talk to me? + if (rx->u.ctrl.data_value_lo || rx->u.ctrl.data_value_hi) { + if ((rx->u.ctrl.data_value_lo != myDroneId[0]) || + (rx->u.ctrl.data_value_hi != myDroneId[1])) { + Debug(1, "Bad DroneID0 %02x %02x\n", rx->u.ctrl.data_value_lo, rx->u.ctrl.data_value_hi); + BadDroneId(); // Bad drone id - disconnect from this tx + result = 1; + } + } + break; + case BK_INFO_DRONEID1: // Does this Tx even want to talk to me? + if (rx->u.ctrl.data_value_lo || rx->u.ctrl.data_value_hi) { + if ((rx->u.ctrl.data_value_lo != myDroneId[2]) || + (rx->u.ctrl.data_value_hi != myDroneId[3])) { + Debug(1, "Bad DroneID1 %02x %02x\n", rx->u.ctrl.data_value_lo, rx->u.ctrl.data_value_hi); + BadDroneId(); // Bad drone id - disconnect from this tx + result = 1; + } + } + break; + default: + break; + }; + } + break; + + case BK_PKT_TYPE_BIND_AUTO: + if (rxaddr == 1) { + if (get_autobind_rssi() > BK_RSSI_DEFAULT) { // Have we disabled autobind using fake RSSI parameter? + Debug(2, "X0"); + break; + } + if (get_autobind_time() == 0) { // Have we disabled autobind using zero time parameter? + Debug(2, "X1"); + break; + } + if (already_bound) { // Do not auto-bind (i.e. to another tx) until we reboot. + Debug(2, "X2"); + break; + } + uint32_t now = AP_HAL::millis(); + if (now < get_autobind_time() * 1000) { // Is this too soon from rebooting/powering up to autobind? + Debug(2, "X3"); + break; + } + // Check the carrier detect to see if the drone is too far away to auto-bind + if (!beken.CarrierDetect()) { + Debug(2, "X4"); + break; + } + result = ProcessBindPacket(rx); + } + break; + + case BK_PKT_TYPE_BIND_MANUAL: // Sent by the tx for a few seconds after power-up when a button is held down + if (rxaddr == 1) { + if (bind_time_ms == 0) { // We have never receiving a binding click + Debug(2, "X5"); + break; // Do not bind + } + if (already_bound) { // Do not manually-bind (i.e. to another tx) until we reboot. + Debug(2, "X6"); + break; + } + // if (uint32_t(AP_HAL::millis() - bind_time_ms) > 1000ul * 60u) // Have we pressed the button to bind recently? One minute timeout + // break; // Do not bind + result = ProcessBindPacket(rx); + } + break; + + case BK_PKT_TYPE_TELEMETRY: + case BK_PKT_TYPE_DFU: + default: + // This is one of our packets! Ignore it. + break; + } + return result; +} + +// ---------------------------------------------------------------------------- +// Prepare to send a FCC packet +void AP_Radio_beken::UpdateFccScan(void) +{ + // Support scan mode + if (beken.fcc.scan_mode) { + beken.fcc.scan_count++; + if (beken.fcc.scan_count >= 200) { + beken.fcc.scan_count = 0; + beken.fcc.channel += 2; // Go up by 2Mhz + if (beken.fcc.channel >= CHANNEL_FCC_HIGH) { + beken.fcc.channel = CHANNEL_FCC_LOW; + } + } + } +} + +// ---------------------------------------------------------------------------- +// main IRQ handler +void AP_Radio_beken::irq_handler(uint32_t when) +{ + if (beken.fcc.fcc_mode) { + // don't process interrupts in FCCTEST mode + beken.WriteReg(BK_WRITE_REG | BK_STATUS, + (BK_STATUS_RX_DR | BK_STATUS_TX_DS | BK_STATUS_MAX_RT)); // clear RX_DR or TX_DS or MAX_RT interrupt flag + return; + } + + // Determine which state fired the interrupt + bool bNext = false; + bool bRx = false; + uint8_t bk_sta = beken.ReadStatus(); + if (bk_sta & BK_STATUS_TX_DS) { + DEBUG1_LOW(); + DEBUG1_HIGH(); + DEBUG1_LOW(); + DEBUG1_HIGH(); + DEBUG1_LOW(); + DEBUG1_HIGH(); + // Packet was sent towards the Tx board + synctm.tx_time_us = when; + beken.SwitchToIdleMode(); + if (beken.fcc.disable_crc_mode && !beken.fcc.disable_crc) { + beken.SetCrcMode(true); + } + bNext = bRx = true; + } + if (bk_sta & BK_STATUS_MAX_RT) { + // We have had a "max retries" error + } + bool bReply = false; + if (bk_sta & BK_STATUS_RX_DR) { + DEBUG1_LOW(); + DEBUG1_HIGH(); + DEBUG1_LOW(); + DEBUG1_HIGH(); + // We have received a packet + uint8_t rxstd = 0; + // Which pipe (address) have we received this packet on? + if ((bk_sta & BK_STATUS_RX_MASK) == BK_STATUS_RX_P_0) { + rxstd = 0; + } else if ((bk_sta & BK_STATUS_RX_MASK) == BK_STATUS_RX_P_1) { + rxstd = 1; + } else { + stats.recv_errors++; + } + bNext = true; + + uint8_t len, fifo_sta; + uint8_t packet[32]; + do { + stats.recv_packets++; + len = beken.ReadReg(BK_R_RX_PL_WID_CMD); // read received packet length in bytes + + if (len <= PACKET_LENGTH_RX_MAX) { + bReply = true; + synctm.Rx(when); + // printf("R%d ", when - next_switch_us); + next_switch_us = when + synctm.sync_time_us + 1500; // Switch channels if we miss the next packet + // This includes short packets (e.g. where no telemetry was sent) + beken.ReadRegisterMulti(BK_RD_RX_PLOAD, packet, len); // read receive payload from RX_FIFO buffer + // DebugPrintf(3, "Packet %d(%d) %d %d %d %d %d %d %d %d ...\r\n", rxstd, len, + // packet[0], packet[1], packet[2], packet[3], packet[4], packet[5], packet[6], packet[7]); + } else { // Packet was too long + beken.ReadRegisterMulti(BK_RD_RX_PLOAD, packet, 32); // read receive payload from RX_FIFO buffer + beken.Strobe(BK_FLUSH_RX); // flush Rx + } + fifo_sta = beken.ReadReg(BK_FIFO_STATUS); // read register FIFO_STATUS's value + } while (!(fifo_sta & BK_FIFO_STATUS_RX_EMPTY)); // while not empty + beken.WriteReg(BK_WRITE_REG | BK_STATUS, + (BK_STATUS_RX_DR | BK_STATUS_TX_DS | BK_STATUS_MAX_RT)); // clear RX_DR or TX_DS or MAX_RT interrupt flag + if (1 == ProcessPacket(packet, rxstd)) { + bNext = false; // Because double binding detected + } + if (beken.fcc.enable_cd) { + beken.fcc.last_cd = beken.CarrierDetect(); // Detect if close or not + } else { + beken.fcc.last_cd = true; // Assumed to be close + } + } + + // Clear the bits + beken.WriteReg((BK_WRITE_REG|BK_STATUS), (BK_STATUS_MAX_RT | BK_STATUS_TX_DS | BK_STATUS_RX_DR)); + if (bReply) { + uint32_t now = AP_HAL::micros(); + uint32_t delta = chTimeUS2I(800 + next_switch_us - now); // Do not use US2ST since that will overflow 32 bits + chSysLock(); + chVTResetI(&timeout_vt); // Stop the normal timeout + chVTSetI(&timeout_vt, delta, trigger_timeout_event, nullptr); // Timeout after 7ms + chSysUnlock(); + + if (get_telem_enable() && have_tx_pps) { // Note that the user can disable telemetry, but the transmitter will be less functional in this case. + bNext = bRx = false; + // Send the telemetry reply to the controller + beken.Strobe(BK_FLUSH_TX); // flush Tx + beken.ClearAckOverflow(); + bool txDfu = UpdateTxData(); + if (txDfu) { + beken.pktDataDfu.channel = syncch.channel; + } else { + beken.pktDataTx.channel = syncch.channel; + } + if (beken.fcc.disable_crc_mode) { + // Only disable the CRC on reception, not transmission, so the connection remains. + beken.SwitchToIdleMode(); + beken.SetCrcMode(false); + } + beken.SwitchToTxMode(); + DEBUG1_LOW(); + hal.scheduler->delay_microseconds(200); // delay to give the (remote) tx a chance to switch to receive mode + DEBUG1_HIGH(); + if (txDfu) { + beken.SendPacket(BK_W_TX_PAYLOAD_NOACK_CMD, (uint8_t *)&beken.pktDataDfu, PACKET_LENGTH_TX_DFU); + } else { + beken.SendPacket(BK_W_TX_PAYLOAD_NOACK_CMD, (uint8_t *)&beken.pktDataTx, PACKET_LENGTH_TX_TELEMETRY); + } + } else { // Try to still work when telemetry is disabled + bNext = true; + } + } + if (bNext) { + nextChannel(1); + } + if (bRx) { + beken.SwitchToRxMode(); // Prepare to receive next packet (on the next channel) + } +} + +// ---------------------------------------------------------------------------- +// handle timeout IRQ (called when we need to switch channels) +void AP_Radio_beken::irq_timeout(uint32_t when) +{ + DEBUG1_LOW(); + DEBUG1_HIGH(); + DEBUG1_LOW(); + DEBUG1_HIGH(); + DEBUG1_LOW(); + DEBUG1_HIGH(); + DEBUG1_LOW(); + DEBUG1_HIGH(); + DEBUG1_LOW(); + DEBUG1_HIGH(); + + if (beken.bkReady) { // We are not reinitialising the chip in the main thread + static uint8_t check_params_timer = 0; + if (++check_params_timer >= 10) { // We don't need to test the parameter logic every ms. + // Every 50ms get here + bool bOldReady = beken.bkReady; + beken.bkReady = false; + check_params_timer = 0; + // Set the transmission power + uint8_t pwr = get_transmit_power(); // 1..8 + if (pwr != beken.fcc.power + 1) { + if ((pwr > 0) && (pwr <= 8)) { + beken.SwitchToIdleMode(); + beken.SetPower(pwr-1); // (this will set beken.fcc.power) + } + } + + // Set CRC mode + uint8_t crc = get_disable_crc(); + if (crc != beken.fcc.disable_crc_mode) { + beken.SwitchToIdleMode(); + beken.SetCrcMode(crc); + beken.fcc.disable_crc_mode = crc; + } + + // Do we need to change our factory test mode? + uint8_t factory = get_factory_test(); + if (factory != beken.fcc.factory_mode) { + beken.SwitchToIdleMode(); + // Set frequency + syncch.channel = factory ? (factory-1) + CHANNEL_COUNT_LOGICAL*CHANNEL_NUM_TABLES : 0; + // Set address + beken.SetFactoryMode(factory); + } + + // Do we need to change our fcc test mode status? + uint8_t fcc = get_fcc_test(); + if (fcc != beken.fcc.fcc_mode) { + beken.Strobe(BK_FLUSH_TX); + if (fcc == 0) { // Turn off fcc test mode + if (beken.fcc.CW_mode) { + beken.SwitchToIdleMode(); + beken.SetCwMode(false); + } + } else { + if (fcc > 3) { + if (!beken.fcc.CW_mode) { + beken.SwitchToIdleMode(); + beken.SetCwMode(true); + beken.DumpRegisters(); + } + } else { + if (beken.fcc.CW_mode) { + beken.SwitchToIdleMode(); + beken.SetCwMode(false); + } + } + switch (fcc) { + case 1: case 4: + default: + beken.fcc.channel = CHANNEL_FCC_LOW; + break; + case 2: case 5: + beken.fcc.channel = CHANNEL_FCC_MID; + break; + case 3: case 6: + beken.fcc.channel = CHANNEL_FCC_HIGH; + break; + }; + } + beken.fcc.fcc_mode = fcc; + DebugPrintf(1, "\r\nFCC mode %d\r\n", fcc); + } + beken.bkReady = bOldReady; + } + + // For fcc mode, just send packets on timeouts (every 5ms) + if (beken.fcc.fcc_mode) { + beken.SwitchToTxMode(); + beken.ClearAckOverflow(); + UpdateFccScan(); + beken.SetChannel(beken.fcc.channel); + UpdateTxData(); + beken.pktDataTx.channel = 0; + if (!beken.fcc.CW_mode) { + beken.SendPacket(BK_WR_TX_PLOAD, (uint8_t *)&beken.pktDataTx, PACKET_LENGTH_TX_TELEMETRY); + } + } else { + // Normal modes - we have timed out for channel hopping + int32_t d = synctm.sync_time_us; // Time between packets, e.g. 5100 us + uint32_t dt = when - synctm.rx_time_us; + if (dt > 50*d) { // We have lost sync (missed 50 packets) so slow down the channel hopping until we resync + d *= 5; // 3 or 5 are relatively prime to the table size of 16. + DebugPrintf(2, "C"); + if (dt > 120*d) { // We have missed 3 seconds - try the safe WiFi table + DebugPrintf(2, "S"); + syncch.SafeTable(); + } + } else { + // DebugPrintf(2, "c%d ", AP_HAL::micros() - next_switch_us); + DebugPrintf(2, "c"); + adaptive.Miss(syncch.channel); + } + { + uint8_t fifo_sta = radio_singleton->beken.ReadReg(BK_FIFO_STATUS); // read register FIFO_STATUS's value + if (!(fifo_sta & BK_FIFO_STATUS_RX_EMPTY)) { // while not empty + DEBUG1_LOW(); + DEBUG1_HIGH(); + DebugPrintf(2, "#"); // We have received a packet, but the interrupt was not triggered! + radio_singleton->irq_handler(next_switch_us); // Use this broken time + DEBUG1_LOW(); + DEBUG1_HIGH(); + } else { + next_switch_us += d; // Switch channels if we miss the next packet + } + } + int32_t ss = int32_t(next_switch_us - when); + if (ss < 1000) { // Not enough time + next_switch_us = when + d; // Switch channels if we miss the next packet + DebugPrintf(2, "j"); + } + beken.SwitchToIdleMode(); + nextChannel(1); // Switch to the next channel + beken.SwitchToRxMode(); + beken.ClearAckOverflow(); + } + } + + // Ask for another timeout + uint32_t now = AP_HAL::micros(); + if (int32_t(next_switch_us - when) < 300) { // Too late for that one + next_switch_us = now + synctm.sync_time_us; + } + if (int32_t(next_switch_us - now) < 250) { // Too late for this one + next_switch_us = now + synctm.sync_time_us; + } + uint32_t delta = chTimeUS2I(next_switch_us - now); // Do not use US2ST since that will overflow 32 bits. + + chSysLock(); + chVTSetI(&timeout_vt, delta, trigger_timeout_event, nullptr); // Timeout every 5 ms + chSysUnlock(); +} + +// ---------------------------------------------------------------------------- +// Thread that supports Beken Radio work triggered by interrupts +// This is the only thread that should access the Beken radio chip via SPI. +void AP_Radio_beken::irq_handler_thd(void *arg) +{ + (void) arg; + while (true) { + DEBUG1_LOW(); + eventmask_t evt = chEvtWaitAny(ALL_EVENTS); + DEBUG1_HIGH(); + if (_irq_handler_ctx != nullptr) { // Sanity check + _irq_handler_ctx->name = "RadioBeken"; // Only useful to be done once but here is done often + } + + radio_singleton->beken.lock_bus(); + switch (evt) { + case EVT_IRQ: + if (radio_singleton->beken.fcc.fcc_mode != 0) { + DebugPrintf(3, "IRQ FCC\n"); + } + radio_singleton->irq_handler(isr_irq_time_us); + break; + case EVT_TIMEOUT: + radio_singleton->irq_timeout(isr_timeout_time_us); + break; + case EVT_BIND: // The user has clicked on the "Start Bind" button on the web interface + DebugPrintf(2, "\r\nBtnStartBind\r\n"); + break; + default: + break; + } + radio_singleton->beken.unlock_bus(); + } +} + +void AP_Radio_beken::setChannel(uint8_t channel) +{ + beken.SetChannel(channel); +} + +const uint8_t bindHopData[256] = { +#if 0 // Support single frequency mode (no channel hopping) + // Normal frequencies + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Normal + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Wifi channel 1,2,3,4,5 + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Wifi channel 6 + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Wifi channel 7 + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Wifi channel 8 + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Wifi channel 9,10,11 + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Test mode channels + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Reserved + // Alternative frequencies + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Normal + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Wifi channel 1,2,3,4,5 + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Wifi channel 6 + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Wifi channel 7 + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Wifi channel 8 + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Wifi channel 9,10,11 + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Test mode channels + 23,23,23,23,23,23,23,23,23,23,23,23,23,23,23,23, // Reserved +#else // Frequency hopping + // Normal frequencies + 47,21,31,52,36,13,72,41, 69,56,16,26,61,10,45,66, // Normal + 57,62,67,72,58,63,68,59, 64,69,60,65,70,61,66,71, // Wifi channel 1,2,3,4,5 (2457..2472MHz) + 62,10,67,72,63,68,11,64, 69,60,65,70,12,61,66,71, // Wifi channel 6 + 10,67,11,72,12,68,13,69, 14,65,15,70,16,66,17,71, // Wifi channel 7 + 10,70,15,20,14,71,16,21, 12,17,22,72,13,18,11,19, // Wifi channel 8 + 10,15,20,25,11,16,21,12, 17,22,13,18,23,14,19,24, // Wifi channel 9,10,11 + 46,41,31,52,36,13,72,69, 21,56,16,26,61,66,10,43, // Test mode channels + 46,41,31,52,36,13,72,69, 21,56,16,26,61,66,10,43, // Reserved + // Alternative frequencies + 17,11,63,19,67,44,43,38, 50,54,70,58,29,35,25,14, // Normal + 18,10,23,21,33,44,41,38, 52,45,47,25,30,35,49,14, // Wifi channel 1,2,3,4,5 + 18,56,23,21,33,44,41,38, 52,45,47,25,30,35,49,14, // Wifi channel 6 + 18,56,23,21,33,44,41,38, 52,45,47,25,30,35,49,61, // Wifi channel 7 + 68,56,24,53,33,44,41,38, 28,45,47,65,30,35,49,61, // Wifi channel 8 + 68,56,72,53,33,44,41,38, 28,45,47,65,30,35,49,61, // Wifi channel 9,10,11 + 46,41,31,52,36,13,72,69, 21,56,16,26,61,66,10,43, // Test mode channels (as normal) + 46,41,31,52,36,13,72,69, 21,56,16,26,61,66,10,43, // Reserved (as normal) +#endif +}; + +void AP_Radio_beken::nextChannel(uint8_t skip) +{ + if (skip) { + syncch.NextChannel(); + } + setChannel(bindHopData[syncch.channel]); +} + +/* + save bind info + */ +void AP_Radio_beken::save_bind_info(void) +{ + // access to storage for bind information + StorageAccess bind_storage(StorageManager::StorageBindInfo); + struct bind_info info; + + info.magic = bind_magic; + info.bindTxId[0] = beken.TX_Address[0]; + info.bindTxId[1] = beken.TX_Address[1]; + info.bindTxId[2] = beken.TX_Address[2]; + info.bindTxId[3] = beken.TX_Address[3]; + info.bindTxId[4] = beken.TX_Address[4]; + bind_storage.write_block(0, &info, sizeof(info)); +} + +/* + load bind info + */ +bool AP_Radio_beken::load_bind_info(void) +{ + // access to storage for bind information + StorageAccess bind_storage(StorageManager::StorageBindInfo); + struct bind_info info; + + if (!bind_storage.read_block(&info, 0, sizeof(info)) || info.magic != bind_magic) { + return false; + } + + beken.SetAddresses(&info.bindTxId[0]); + + return true; +} + +// ---------------------------------------------------------------------------- +void AP_Radio_beken::BadDroneId(void) +{ + if (stats.recv_packets >= 1000) { // We are already chatting to this TX for some time. + return; // Do not disconnect from it. + } + + // clear the current bind information + valid_connection = false; + // with luck we will connect to another tx + beken.SwitchToIdleMode(); + beken.SetFactoryMode(0); // Reset the tx address + adaptive.Invalidate(); + syncch.SetHopping(0,0); + already_bound = false; // Not already solidly bound to a drone + stats.recv_packets = 0; + beken.WriteReg(BK_WRITE_REG|BK_EN_RXADDR, 0x02); + have_tx_pps = false; +} + +// ---------------------------------------------------------------------------- +// Which bits correspond to each channel within a table, for adaptive frequencies +static const uint8_t channel_bit_table[CHANNEL_COUNT_LOGICAL] = { + 0x01, 0, 0x02, 0, 0x04, 0, 0x08, 0, + 0x10, 0, 0x20, 0, 0x40, 0, 0x80, 0 +}; + +// Step through the channels +void SyncChannel::NextChannel(void) +{ + channel &= 0x7f; + if (channel >= CHANNEL_COUNT_LOGICAL*CHANNEL_NUM_TABLES) { + // We are in the factory test modes. Keep the channel as is. + } else { + if (countdown != countdown_invalid) { + if (--countdown == 0) { + channel = countdown_chan; + countdown = countdown_invalid; + hopping_current = hopping_wanted = 0; + return; + } + } else if (hopping_countdown != countdown_invalid) { + if (--hopping_countdown == 0) { + hopping_current = hopping_wanted; + hopping_countdown = countdown_invalid; + // printf("{Use %d} ", hopping_current); + } + } + uint8_t table = channel / CHANNEL_COUNT_LOGICAL; + channel = (channel + 1) % CHANNEL_COUNT_LOGICAL; + channel += table * CHANNEL_COUNT_LOGICAL; + // Support adaptive frequency hopping + if (hopping_current & channel_bit_table[channel % CHANNEL_COUNT_LOGICAL]) { + channel |= 0x80; + } + } +} + +// If we have not received any packets for ages, try a WiFi table that covers all frequencies +void SyncChannel::SafeTable(void) +{ + channel &= 0x7f; + if (channel >= CHANNEL_COUNT_LOGICAL*CHANNEL_NUM_TABLES) { + // We are in the factory test modes. Reset to default table. + channel = 0; + } else { + uint8_t table = channel / CHANNEL_COUNT_LOGICAL; + if ((table != CHANNEL_BASE_TABLE) && (table != CHANNEL_SAFE_TABLE)) { // Are we using a table that is high end or low end only? + channel %= CHANNEL_COUNT_LOGICAL; + channel += CHANNEL_SAFE_TABLE * CHANNEL_COUNT_LOGICAL; + } + } +} + +// Check if valid channel index; we have received a packet describing the current channel index +void SyncChannel::SetChannelIfSafe(uint8_t chan) +{ + if (channel != chan) { + DebugPrintf(2, "{{%d}} ", chan); + } + chan &= 0x7f; // Disregard hopping + if (chan >= CHANNEL_COUNT_LOGICAL*CHANNEL_NUM_TABLES) { + if (chan == lastchan) { + channel = chan; // Allow test mode channels if two in a row + } else { + chan = 0; // Disallow test mode tables unless followed by each other + } + lastchan = chan; + } else { + lastchan = 0; + } + channel = chan; +} + +// We have received a packet on this channel +void SyncAdaptive::Get(uint8_t channel) +{ + uint8_t f = bindHopData[channel]; + rx[f]++; +} + +enum { ADAPT_THRESHOLD = 50 }; // Missed packets threshold for adapting the hopping + +// We have missed a packet on this channel. Consider adapting. +void SyncAdaptive::Miss(uint8_t channel) +{ + uint8_t f1 = bindHopData[channel]; + missed[f1]++; + uint8_t f2 = bindHopData[channel ^ 0x80]; + int32_t delta1 = missed[f1] - rx[f1]; + int32_t delta2 = missed[f2] - rx[f2]; + if ((delta1 > ADAPT_THRESHOLD) && // Worse than 50% reception on this channel + (delta1 > delta2)) { + // Ok consider swapping this channel + uint8_t bit = channel_bit_table[channel % CHANNEL_COUNT_LOGICAL]; + if (bit) { // Is an even packet + uint8_t oh = hopping; + if (channel & 0x80) { // Swap back from alternative + hopping &= ~bit; + } else { // Swap to alternative + hopping |= bit; + } + if (hopping != oh) { // Have we changed? + missed[f2] = rx[f2] = 0; // Reset the values + // printf("{%d->%d:%d} ", f1+2400, f2+2400, hopping); + } + } + } +} + + + +#endif // HAL_RCINPUT_WITH_AP_RADIO + diff --git a/libraries/AP_Radio/AP_Radio_bk2425.h b/libraries/AP_Radio/AP_Radio_bk2425.h new file mode 100644 index 0000000000..82a68abf41 --- /dev/null +++ b/libraries/AP_Radio/AP_Radio_bk2425.h @@ -0,0 +1,261 @@ +/* + 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, see . + */ +#pragma once + +/* + AP_Radio implementation for CC2500 2.4GHz radio. + + With thanks to cleanflight and betaflight projects + */ + +#include "AP_Radio_backend.h" + +#if defined(HAL_RCINPUT_WITH_AP_RADIO) && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 +#include "hal.h" +#include "telem_structure.h" +#include "driver_bk2425.h" + +#define BEKEN_MAX_CHANNELS 16 +// Documentation of the expected RSSI values. These are determined by the Cypress chip. +enum { + BK_RSSI_MIN = 0, // Minimum value for RSSI + BK_RSSI_DEFAULT = 16, // The default value for RSSI for chips that do not support it. + BK_RSSI_MAX = 31 // Maximum value for RSSI +}; + +// This helper struct estimates the times (in microseconds) between packets, +// according to the STM32 clock which may well be 2% different from the STM8 clock. +// For instance it may be 5108 instead of the nominal 5000 microseconds. +struct SyncTiming { + enum { TARGET_DELTA_RX = 5000, // Nominal 5ms between packets is expected + SLOP_DELTA_RX = TARGET_DELTA_RX / 10, // +/- 500us i.e. 10% skew each way is accepted. + DIFF_DELTA_RX = TARGET_DELTA_RX / 100 + }; // Two consequetive deltas must be very close together (50us) + uint32_t packet_timer; // Time we last received a valid control packet + uint32_t rx_time_us; // Time we last received a packet + uint32_t tx_time_us; // Time we last finished transmitting a packet + uint32_t delta_rx_time_us; // Time between last rx packets + uint32_t last_delta_rx_time_us; // previous version of the delta + uint32_t sync_time_us; // Estimate of base time in microseconds between packets. 5000 +/- 500 + SyncTiming() : // Constructor to setup sensible initial conditions + delta_rx_time_us(TARGET_DELTA_RX), + last_delta_rx_time_us(TARGET_DELTA_RX), + sync_time_us(TARGET_DELTA_RX) + {} + void Rx(uint32_t when); // Adjust the timing based on a new packet +}; + +// Helper struct for synchronising channels when we change hopping table (e.g. learn of a WiFi channel change). +struct SyncChannel { + enum { countdown_invalid = 0 }; // When countdown is this value, no change is pending + uint8_t channel; // Index within the channel hopping sequence. Corresponds to txChannel on the button board + uint8_t lastchan; // Last requested index, if it is a factory test channel. + uint8_t countdown; // How many packet slots until a pending table change occurs? + uint8_t countdown_chan; // Which channel do we jump to when the table change happens? + uint8_t hopping_current; // Which alternative channels are we on now + uint8_t hopping_wanted; // Which alternative channels will we be on when Tx changes over? + uint8_t hopping_countdown; // How many packet slots until a pending table change occurs? + SyncChannel() : // Constructor to setup sensible initial conditions + channel(0), + lastchan(0), + countdown(countdown_invalid), + countdown_chan(0), + hopping_current(0), + hopping_wanted(0), + hopping_countdown(countdown_invalid) + {} + void SetChannelIfSafe(uint8_t chan); // Check if valid channel index; we have received a packet describing the current channel index + void SetChannel(uint8_t chan) // Already safe. We have received a packet describing the current channel index + { + channel = chan; + } + void SetCountdown(uint8_t cnt, uint8_t nextCh) // We receive a countdown to a non-normal channel change in the future + { + countdown = cnt; + countdown_chan = nextCh; + } + void SetHopping(uint8_t cnt, uint8_t nextHopping) // We receive a countdown to a change in the adaptive table in the future/now + { + hopping_countdown = cnt; + hopping_wanted = nextHopping; + if (cnt == 0) { + hopping_current = nextHopping; + } + } + void NextChannel(void); // Step through the channels normally (taking countdowns into account) + void SafeTable(void); // Give up on this WiFi table as packets have not been received +}; + +// This helper struct determines which physical channels are better +struct SyncAdaptive { + uint32_t missed[CHANNEL_FCC_HIGH+1]; // Missed + uint32_t rx[CHANNEL_FCC_HIGH+1]; // Received + uint8_t hopping; // Currently wanted hopping state. Send this to the tx. + SyncAdaptive() : // Constructor to setup sensible initial conditions + hopping(0) + {} + void Miss(uint8_t channel); + void Get(uint8_t channel); + void Invalidate() + { + hopping = 0; // e.g. if we have jumped tables + } +}; + +// Support OTA upload. Assumes that mavlink offsets go from zero upwards contiguously +struct FwUpload { + enum { SZ_BUFFER = 128 }; // Must be a power of two + mavlink_channel_t chan; // Reference for talking to mavlink subsystem + uint8_t counter; // Used to throttle the upload, to prevent starvation of telemetry + enum telem_type fw_type; // Whether we are uploading program code or a test tune + + // Data that is reset by reset() + bool need_ack; // When true, we need to talk to mavlink subsystem (ask for more firmware) + uint32_t added; // The number of bytes added to the queue + uint32_t sent; // The number of bytes sent to the tx + uint32_t acked; // The number of bytes acked by the tx + bool rx_ack; // True each time we receive a non-zero ack from the tx + bool rx_reboot; // True when we are in the rebooting process + uint8_t pending_data[SZ_BUFFER]; // Pending data (from mavlink packets) circular buffer + uint8_t pending_head; // Where mavlink packets are added (relative to pending_data[0]) + uint8_t pending_tail; // Where DFU packets are taken from (relative to pending_data[0]) + uint16_t file_length; // The length of the file, six more than the value stored in the first 16 bit word + uint16_t file_length_round; // file_length rounded up to 0x80 + + // Helper functions + uint8_t pending_length() + { + return (pending_head - pending_tail) & (SZ_BUFFER-1); + } + uint8_t free_length() + { + return SZ_BUFFER - 1 - pending_length(); // Do not fill in the last byte in the circular buffer + } + void queue(const uint8_t *pSrc, uint8_t len); // Assumes sufficient room has been checked for + void dequeue(uint8_t *pDst, uint8_t len); // Assumes sufficient data has been checked for + void reset() + { + file_length = file_length_round = 0; + added = sent = acked = 0; + pending_head = pending_tail = 0; + rx_reboot = rx_ack = need_ack = false; + } +}; + +// Main class for receiving (and replying) to Beken radio packets +class AP_Radio_beken : public AP_Radio_backend +{ +public: + // Override base class functions + AP_Radio_beken(AP_Radio &radio); // Normal constructore + bool init(void) override; // initialise the radio + bool reset(void) override; // reset the radio + bool send(const uint8_t *pkt, uint16_t len) override; // send a packet + void start_recv_bind(void) override; // start bind process as a receiver + uint32_t last_recv_us(void) override; // return time in microseconds of last received R/C packet + uint8_t num_channels(void) override; // return number of input channels + uint16_t read(uint8_t chan) override; // return current "PWM" (value) of a channel + void handle_data_packet(mavlink_channel_t chan, const mavlink_data96_t &m) override; // handle a data96 mavlink packet for fw upload + void update(void) override; // update status + + uint32_t get_tx_version(void) override // get TX fw version + { + // pack date into 16 bits for vendor_id in AUTOPILOT_VERSION + return (uint16_t(tx_date.firmware_year)<<12) + (uint16_t(tx_date.firmware_month)<<8) + tx_date.firmware_day; + } + const AP_Radio::stats &get_stats(void) override; // get radio statistics structure + + // Extra public functions + void set_wifi_channel(uint8_t channel) override + { + t_status.wifi_chan = channel; // set the 2.4GHz wifi channel used by companion computer, so it can be avoided + } + +private: + // Static functions, for interrupt support + static void irq_handler_thd(void* arg); + static void trigger_irq_radio_event(void); + static void trigger_timeout_event(void *arg); + + // Private functions + void radio_init(void); + uint8_t ProcessPacket(const uint8_t* packet, uint8_t rxaddr); + uint8_t ProcessBindPacket(const packetFormatRx * rx); + void BadDroneId(void); // The tx we are listening to wants to talk to another drone + void setChannel(uint8_t channel); + void nextChannel(uint8_t skip); + uint16_t calc_crc(uint8_t *data, uint8_t len); + void irq_handler(uint32_t when); + void irq_timeout(uint32_t when); + void save_bind_info(void); + bool load_bind_info(void); + void UpdateFccScan(void); + bool UpdateTxData(void); + void map_stick_mode(void); // Support mode1,2,3,4 for stick mapping + void update_SRT_telemetry(void); + void check_fw_ack(void); + + // Static data, for interrupt support + friend class SyncChannel; // For DebugPrintf support + static AP_Radio_beken *radio_singleton; // Singleton pointer to the Beken radio instance + static thread_t *_irq_handler_ctx; + static virtual_timer_t timeout_vt; + static uint32_t isr_irq_time_us; // Time the Beken IRQ was last triggered, in the handler interrupts (in microseconds) + static uint32_t isr_timeout_time_us; // Time the timeout was last triggered (copied from irq_time_us via irq_when_us) (in microseconds) + static uint32_t next_switch_us; // Time when we next want to switch radio channels (in microseconds) + static uint32_t bind_time_ms; // Rough time in ms (milliseconds) when the last BIND command was received + + // Class data + AP_HAL::OwnPtr dev; // Low level support of SPI device + HAL_Semaphore sem; // semaphore between ISR and main thread to protect fwupload + + AP_Radio::stats stats; // Radio stats (live) for the current time-period + AP_Radio::stats last_stats; // Radio stats (snapshot) for the previous time-period + uint16_t pwm_channels[BEKEN_MAX_CHANNELS]; // Channel data + uint8_t chan_count; // Number of valid channels + + Radio_Beken beken; // The low level class for communicating to the Beken chip + SyncChannel syncch; // Index within the channel hopping sequence. Corresponds to txChannel on the button board + static SyncTiming synctm; // Timing between packets, according to the local clock (not the tx clock). + uint32_t already_bound; // True when we have received packets from a tx after bootup. Prevent auto-binding to something else. + FwUpload fwupload; // Support OTA upload + SyncAdaptive adaptive; // Support adaptive hopping + struct { + uint8_t firmware_year; + uint8_t firmware_month; + uint8_t firmware_day; + } tx_date; + + // Bind structure saved to storage + static const uint16_t bind_magic = 0x120a; + struct PACKED bind_info { + uint16_t magic; + uint8_t bindTxId[5]; // The transmission address I last used + }; + + // Received + struct telem_status t_status; // Keep track of certain data that can be sent as telemetry to the tx. + uint32_t last_pps_ms; // Timestamp of the last PPS (packets per second) calculation, in milliseconds. + uint32_t tx_pps; // Last telemetry PPS received from Tx + uint32_t have_tx_pps; // 0=never received, 1=received at least one, 2=received recently + uint32_t valid_connection; // Take some time before admitting to ardupilot we have a connection + uint32_t telem_send_count; // How many telemetry packets have i sent? + + // Parameters + ITX_SPEED spd; // Speed of radio modulation. + uint8_t myDroneId[4]; // CRC of the flight boards UUID, to inform the tx +}; + +#endif // HAL_RCINPUT_WITH_AP_RADIO diff --git a/libraries/AP_Radio/AP_Radio_cc2500.cpp b/libraries/AP_Radio/AP_Radio_cc2500.cpp index 38b4058172..943ed107c0 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.cpp +++ b/libraries/AP_Radio/AP_Radio_cc2500.cpp @@ -18,10 +18,10 @@ #include #include #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -static THD_WORKING_AREA(_irq_handler_wa, 512); -#define TIMEOUT_PRIORITY 250 //Right above timer thread +#define TIMEOUT_PRIORITY 185 #define EVT_TIMEOUT EVENT_MASK(0) #define EVT_IRQ EVENT_MASK(1) #define EVT_BIND EVENT_MASK(2) @@ -29,9 +29,7 @@ static THD_WORKING_AREA(_irq_handler_wa, 512); extern const AP_HAL::HAL& hal; -#define Debug(level, fmt, args...) do { if ((level) <= get_debug_level()) { hal.console->printf(fmt, ##args); }} while (0) - -#define LP_FIFO_SIZE 16 // Physical data FIFO lengths in Radio +#define Debug(level, fmt, args...) do { if ((level) <= get_debug_level()) { gcs().send_text(MAV_SEVERITY_INFO, fmt, ##args); }} while (0) // object instance for trampoline AP_Radio_cc2500 *AP_Radio_cc2500::radio_singleton; @@ -41,6 +39,33 @@ virtual_timer_t AP_Radio_cc2500::timeout_vt; uint32_t AP_Radio_cc2500::irq_time_us; #endif +#define USE_D16_FORMAT 0 + +/* + we are setup for a channel spacing of 0.3MHz, with channel 0 being 2403.6MHz + + For D16 protocol we select 47 channels from a max of 235 channels + + For SRT protocol we select 23 channels from a max of 235 channels, + and avoid channels near to the WiFi channel of the Sonix video board + */ +#if USE_D16_FORMAT +#define NUM_CHANNELS 47 +#define MAX_CHANNEL_NUMBER 0xEB +#define INTER_PACKET_MS 9 +#define INTER_PACKET_INITIAL_MS (INTER_PACKET_MS+2) +#define PACKET_SENT_DELAY_US 3300 +#else +#define NUM_CHANNELS 23 +#define MAX_CHANNEL_NUMBER 0xEB +#define INTER_PACKET_MS 9 +#define INTER_PACKET_INITIAL_MS (INTER_PACKET_MS+5) +#define PACKET_SENT_DELAY_US 2800 +#endif + +#define SEARCH_START_PKTS 40 +#define AUTOBIND_CHANNEL 100 + /* constructor */ @@ -58,16 +83,18 @@ AP_Radio_cc2500::AP_Radio_cc2500(AP_Radio &_radio) : bool AP_Radio_cc2500::init(void) { #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - if(_irq_handler_ctx != nullptr) { + if (_irq_handler_ctx != nullptr) { AP_HAL::panic("AP_Radio_cc2500: double instantiation of irq_handler\n"); } chVTObjectInit(&timeout_vt); - _irq_handler_ctx = chThdCreateStatic(_irq_handler_wa, - sizeof(_irq_handler_wa), - TIMEOUT_PRIORITY, /* Initial priority. */ - irq_handler_thd, /* Thread function. */ - NULL); /* Thread parameter. */ + _irq_handler_ctx = chThdCreateFromHeap(NULL, + THD_WORKING_AREA_SIZE(2048), + "radio_cc2500", + TIMEOUT_PRIORITY, + irq_handler_thd, + NULL); #endif + return reset(); } @@ -99,7 +126,7 @@ const AP_Radio::stats &AP_Radio_cc2500::get_stats(void) */ uint16_t AP_Radio_cc2500::read(uint8_t chan) { - if (chan >= CC2500_MAX_CHANNELS) { + if (chan >= CC2500_MAX_PWM_CHANNELS) { return 0; } return pwm_channels[chan]; @@ -112,7 +139,7 @@ void AP_Radio_cc2500::update(void) { check_fw_ack(); } - + /* return number of active channels @@ -143,13 +170,16 @@ uint8_t AP_Radio_cc2500::num_channels(void) pwm_channels[chan-1] = tx_pps; chan_count = MAX(chan_count, chan); } - + pwm_channels[11] = (stats.recv_packets % 1000); + chan_count = MAX(chan_count, 12); + if (now - last_pps_ms > 1000) { last_pps_ms = now; t_status.pps = stats.recv_packets - last_stats.recv_packets; last_stats = stats; if (lost != 0 || timeouts != 0) { - Debug(3,"lost=%u timeouts=%u TS=%u\n", unsigned(lost), unsigned(timeouts), sizeof(struct telem_packet_cc2500)); + Debug(timeouts!=0?2:3,"lost=%u timeouts=%u TS=%u\n", + unsigned(lost), unsigned(timeouts), sizeof(struct telem_packet_cc2500)); } lost=0; timeouts=0; @@ -174,25 +204,28 @@ bool AP_Radio_cc2500::send(const uint8_t *pkt, uint16_t len) return false; } -const AP_Radio_cc2500::config AP_Radio_cc2500::radio_config[] = { +const AP_Radio_cc2500::config AP_Radio_cc2500::radio_config_GFSK[] = { + /* + radio config for GFSK with 57kHz deviation + */ {CC2500_00_IOCFG2, 0x01}, // GD2 high on RXFIFO filled or end of packet - {CC2500_17_MCSM1, 0x0C}, // stay in RX on packet receive, CCA always, TX -> IDLE - {CC2500_18_MCSM0, 0x18}, // XOSC expire 64, cal on IDLE -> TX or RX - {CC2500_06_PKTLEN, 0x1E}, // packet length 30 - {CC2500_07_PKTCTRL1, 0x04}, // enable RSSI+LQI, no addr check, no autoflush, PQT=0 - {CC2500_08_PKTCTRL0, 0x01}, // var length mode, no CRC, FIFO enable, no whitening - {CC2500_3E_PATABLE, 0xFF}, // ?? what are we doing to PA table here? + {CC2500_17_MCSM1, 0x03}, // RX->IDLE, CCA always, TX -> IDLE + {CC2500_18_MCSM0, 0x08}, // XOSC expire 64, cal never + {CC2500_06_PKTLEN, 0x0D}, // packet length 13 + {CC2500_07_PKTCTRL1, 0x0C}, // enable RSSI+LQI, no addr check, CRC autoflush, PQT=0 + {CC2500_08_PKTCTRL0, 0x44}, // fixed length mode, CRC, FIFO enable, whitening + {CC2500_3E_PATABLE, 0xFF}, // initially max power {CC2500_0B_FSCTRL1, 0x0A}, // IF=253.90625kHz assuming 26MHz crystal {CC2500_0C_FSCTRL0, 0x00}, // freqoffs = 0 {CC2500_0D_FREQ2, 0x5C}, // freq control high {CC2500_0E_FREQ1, 0x76}, // freq control middle {CC2500_0F_FREQ0, 0x27}, // freq control low - {CC2500_10_MDMCFG4, 0x7B}, // data rate control - {CC2500_11_MDMCFG3, 0x61}, // data rate control + {CC2500_10_MDMCFG4, 0x8C}, // filter bandwidth 203kHz + {CC2500_11_MDMCFG3, 0x2F}, // data rate 120kbaud {CC2500_12_MDMCFG2, 0x13}, // 30/32 sync word bits, no manchester, GFSK, DC filter enabled - {CC2500_13_MDMCFG1, 0x23}, // chan spacing exponent 3, preamble 4 bytes, FEC disabled + {CC2500_13_MDMCFG1, 0xA3}, // chan spacing exponent 3, preamble 4 bytes, FEC enabled {CC2500_14_MDMCFG0, 0x7A}, // chan spacing 299.926757kHz for 26MHz crystal - {CC2500_15_DEVIATN, 0x51}, // modem deviation 25.128906kHz for 26MHz crystal + {CC2500_15_DEVIATN, 0x51}, // modem deviation 57kHz for 26MHz crystal {CC2500_19_FOCCFG, 0x16}, // frequency offset compensation {CC2500_1A_BSCFG, 0x6C}, // bit sync config {CC2500_1B_AGCCTRL2, 0x43}, // target amplitude 33dB @@ -212,41 +245,94 @@ const AP_Radio_cc2500::config AP_Radio_cc2500::radio_config[] = { {CC2500_09_ADDR, 0x00}, // device address 0 (broadcast) }; -const uint16_t CRCTable[] = { - 0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf, - 0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7, - 0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e, - 0x9cc9,0x8d40,0xbfdb,0xae52,0xdaed,0xcb64,0xf9ff,0xe876, - 0x2102,0x308b,0x0210,0x1399,0x6726,0x76af,0x4434,0x55bd, - 0xad4a,0xbcc3,0x8e58,0x9fd1,0xeb6e,0xfae7,0xc87c,0xd9f5, - 0x3183,0x200a,0x1291,0x0318,0x77a7,0x662e,0x54b5,0x453c, - 0xbdcb,0xac42,0x9ed9,0x8f50,0xfbef,0xea66,0xd8fd,0xc974, - 0x4204,0x538d,0x6116,0x709f,0x0420,0x15a9,0x2732,0x36bb, - 0xce4c,0xdfc5,0xed5e,0xfcd7,0x8868,0x99e1,0xab7a,0xbaf3, - 0x5285,0x430c,0x7197,0x601e,0x14a1,0x0528,0x37b3,0x263a, - 0xdecd,0xcf44,0xfddf,0xec56,0x98e9,0x8960,0xbbfb,0xaa72, - 0x6306,0x728f,0x4014,0x519d,0x2522,0x34ab,0x0630,0x17b9, - 0xef4e,0xfec7,0xcc5c,0xddd5,0xa96a,0xb8e3,0x8a78,0x9bf1, - 0x7387,0x620e,0x5095,0x411c,0x35a3,0x242a,0x16b1,0x0738, - 0xffcf,0xee46,0xdcdd,0xcd54,0xb9eb,0xa862,0x9af9,0x8b70, - 0x8408,0x9581,0xa71a,0xb693,0xc22c,0xd3a5,0xe13e,0xf0b7, - 0x0840,0x19c9,0x2b52,0x3adb,0x4e64,0x5fed,0x6d76,0x7cff, - 0x9489,0x8500,0xb79b,0xa612,0xd2ad,0xc324,0xf1bf,0xe036, - 0x18c1,0x0948,0x3bd3,0x2a5a,0x5ee5,0x4f6c,0x7df7,0x6c7e, - 0xa50a,0xb483,0x8618,0x9791,0xe32e,0xf2a7,0xc03c,0xd1b5, - 0x2942,0x38cb,0x0a50,0x1bd9,0x6f66,0x7eef,0x4c74,0x5dfd, - 0xb58b,0xa402,0x9699,0x8710,0xf3af,0xe226,0xd0bd,0xc134, - 0x39c3,0x284a,0x1ad1,0x0b58,0x7fe7,0x6e6e,0x5cf5,0x4d7c, - 0xc60c,0xd785,0xe51e,0xf497,0x8028,0x91a1,0xa33a,0xb2b3, - 0x4a44,0x5bcd,0x6956,0x78df,0x0c60,0x1de9,0x2f72,0x3efb, - 0xd68d,0xc704,0xf59f,0xe416,0x90a9,0x8120,0xb3bb,0xa232, - 0x5ac5,0x4b4c,0x79d7,0x685e,0x1ce1,0x0d68,0x3ff3,0x2e7a, - 0xe70e,0xf687,0xc41c,0xd595,0xa12a,0xb0a3,0x8238,0x93b1, - 0x6b46,0x7acf,0x4854,0x59dd,0x2d62,0x3ceb,0x0e70,0x1ff9, - 0xf78f,0xe606,0xd49d,0xc514,0xb1ab,0xa022,0x92b9,0x8330, - 0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78 +const AP_Radio_cc2500::config AP_Radio_cc2500::radio_config[] = { + /* config for both TX and RX (from SmartRF Studio) + setup for MSK at 120kbaud, FEC enabled, whitening enabled, base freq 2403.999756MHz + channel spacing 299.926758, crystal 26MHz, RX filter bw 203.125kHz + */ + {CC2500_06_PKTLEN, 0x0D}, + {CC2500_07_PKTCTRL1, 0x0C}, + {CC2500_08_PKTCTRL0, 0x44}, + {CC2500_0B_FSCTRL1, 0x0A}, + {CC2500_0D_FREQ2, 0x5C}, + {CC2500_0E_FREQ1, 0x76}, + {CC2500_0F_FREQ0, 0x27}, + {CC2500_11_MDMCFG3, 0x2F}, + {CC2500_12_MDMCFG2, 0x73}, + {CC2500_13_MDMCFG1, 0xA3}, + {CC2500_14_MDMCFG0, 0x7A}, + {CC2500_15_DEVIATN, 0x70}, + {CC2500_17_MCSM1, 0x03}, + {CC2500_18_MCSM0, 0x08}, + {CC2500_19_FOCCFG, 0x16}, + {CC2500_1B_AGCCTRL2, 0x43}, + {CC2500_23_FSCAL3, 0xEA}, + {CC2500_25_FSCAL1, 0x00}, + {CC2500_26_FSCAL0, 0x11}, + {CC2500_2B_AGCTEST, 0x3E}, + {CC2500_03_FIFOTHR, 0x07}, // TX fifo threashold 33, RX fifo threshold 32 + + // config specific to RX + {CC2500_00_IOCFG2, 0x01}, // GD2 high on RXFIFO filled or end of packet + {CC2500_17_MCSM1, 0x03}, // RX->IDLE, CCA always, TX -> IDLE + {CC2500_18_MCSM0, 0x08}, // XOSC expire 64, cal never + {CC2500_3E_PATABLE, 0xFF}, // initially max power }; +const uint16_t CRCTable[] = { + 0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf, + 0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7, + 0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e, + 0x9cc9,0x8d40,0xbfdb,0xae52,0xdaed,0xcb64,0xf9ff,0xe876, + 0x2102,0x308b,0x0210,0x1399,0x6726,0x76af,0x4434,0x55bd, + 0xad4a,0xbcc3,0x8e58,0x9fd1,0xeb6e,0xfae7,0xc87c,0xd9f5, + 0x3183,0x200a,0x1291,0x0318,0x77a7,0x662e,0x54b5,0x453c, + 0xbdcb,0xac42,0x9ed9,0x8f50,0xfbef,0xea66,0xd8fd,0xc974, + 0x4204,0x538d,0x6116,0x709f,0x0420,0x15a9,0x2732,0x36bb, + 0xce4c,0xdfc5,0xed5e,0xfcd7,0x8868,0x99e1,0xab7a,0xbaf3, + 0x5285,0x430c,0x7197,0x601e,0x14a1,0x0528,0x37b3,0x263a, + 0xdecd,0xcf44,0xfddf,0xec56,0x98e9,0x8960,0xbbfb,0xaa72, + 0x6306,0x728f,0x4014,0x519d,0x2522,0x34ab,0x0630,0x17b9, + 0xef4e,0xfec7,0xcc5c,0xddd5,0xa96a,0xb8e3,0x8a78,0x9bf1, + 0x7387,0x620e,0x5095,0x411c,0x35a3,0x242a,0x16b1,0x0738, + 0xffcf,0xee46,0xdcdd,0xcd54,0xb9eb,0xa862,0x9af9,0x8b70, + 0x8408,0x9581,0xa71a,0xb693,0xc22c,0xd3a5,0xe13e,0xf0b7, + 0x0840,0x19c9,0x2b52,0x3adb,0x4e64,0x5fed,0x6d76,0x7cff, + 0x9489,0x8500,0xb79b,0xa612,0xd2ad,0xc324,0xf1bf,0xe036, + 0x18c1,0x0948,0x3bd3,0x2a5a,0x5ee5,0x4f6c,0x7df7,0x6c7e, + 0xa50a,0xb483,0x8618,0x9791,0xe32e,0xf2a7,0xc03c,0xd1b5, + 0x2942,0x38cb,0x0a50,0x1bd9,0x6f66,0x7eef,0x4c74,0x5dfd, + 0xb58b,0xa402,0x9699,0x8710,0xf3af,0xe226,0xd0bd,0xc134, + 0x39c3,0x284a,0x1ad1,0x0b58,0x7fe7,0x6e6e,0x5cf5,0x4d7c, + 0xc60c,0xd785,0xe51e,0xf497,0x8028,0x91a1,0xa33a,0xb2b3, + 0x4a44,0x5bcd,0x6956,0x78df,0x0c60,0x1de9,0x2f72,0x3efb, + 0xd68d,0xc704,0xf59f,0xe416,0x90a9,0x8120,0xb3bb,0xa232, + 0x5ac5,0x4b4c,0x79d7,0x685e,0x1ce1,0x0d68,0x3ff3,0x2e7a, + 0xe70e,0xf687,0xc41c,0xd595,0xa12a,0xb0a3,0x8238,0x93b1, + 0x6b46,0x7acf,0x4854,0x59dd,0x2d62,0x3ceb,0x0e70,0x1ff9, + 0xf78f,0xe606,0xd49d,0xc514,0xb1ab,0xa022,0x92b9,0x8330, + 0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78 +}; + +/* + static probe function for radio auto-detect + */ +bool AP_Radio_cc2500::probe(void) +{ + auto dev = hal.spi->get_device("cc2500"); + if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return false; + } + uint8_t r1=0, r2=0; + if (!dev->read_registers(CC2500_30_PARTNUM | CC2500_READ_BURST | CC2500_READ_SINGLE, &r1, 1) || r1 != 0x80 || + !dev->read_registers(CC2500_31_VERSION | CC2500_READ_BURST | CC2500_READ_SINGLE, &r2, 1) || r2 != 0x03) { + dev->get_semaphore()->give(); + return false; + } + dev->get_semaphore()->give(); + return true; +} + /* initialise the radio */ @@ -262,44 +348,63 @@ void AP_Radio_cc2500::radio_init(void) cc2500.Reset(); hal.scheduler->delay_microseconds(100); - for (uint8_t i=0; idelay_microseconds(900); - calData[c][0] = cc2500.ReadReg(CC2500_23_FSCAL3); - calData[c][1] = cc2500.ReadReg(CC2500_24_FSCAL2); - calData[c][2] = cc2500.ReadReg(CC2500_25_FSCAL1); - } - hal.scheduler->delay_microseconds(10*1000); - + // setup handler for rising edge of IRQ pin -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS hal.gpio->attach_interrupt(HAL_GPIO_RADIO_IRQ, trigger_irq_radio_event, AP_HAL::GPIO::INTERRUPT_RISING); -#endif + + // fill in rxid for use in double bind prevention + char sysid[40] {}; + hal.util->get_system_id(sysid); + uint16_t sysid_crc = calc_crc((const uint8_t *)sysid, strnlen(sysid, sizeof(sysid))); + if (sysid_crc == 0) { + sysid_crc = 1; + } + t_status.rxid[0] = sysid_crc & 0xFF; + t_status.rxid[1] = sysid_crc >> 8; initTuneRx(); if (load_bind_info()) { Debug(3,"Loaded bind info\n"); - listLength = 47; - initialiseData(0); - protocolState = STATE_SEARCH; - chanskip = 1; - nextChannel(1); } else { - protocolState = STATE_BIND_TUNING; + listLength = NUM_CHANNELS; + bindOffset = 0; + setup_hopping_table_SRT(); } - chVTSet(&timeout_vt, chTimeMS2I(10), trigger_timeout_event, nullptr); + uint8_t factory_test = get_factory_test(); + if (factory_test != 0) { + bindTxId[0] = uint8_t(factory_test * 17); + bindTxId[1] = uint8_t(~bindTxId[0]); + setup_hopping_table_SRT(); + } + + // we go straight into search, and rely on autobind + initialiseData(0); + protocolState = STATE_SEARCH; + packet_timer = AP_HAL::micros(); + chanskip = 1; + nextChannel(1); + + // set default autobind power to suit the cc2500 + AP_Param::set_default_by_name("BRD_RADIO_ABLVL", 90); + + chVTSet(&timeout_vt, chTimeMS2I(INTER_PACKET_INITIAL_MS), trigger_timeout_event, nullptr); + } void AP_Radio_cc2500::trigger_irq_radio_event() @@ -316,14 +421,13 @@ void AP_Radio_cc2500::trigger_timeout_event(void *arg) (void)arg; //we are called from ISR context chSysLockFromISR(); - chVTSetI(&timeout_vt, chTimeMS2I(10), trigger_timeout_event, nullptr); + chVTSetI(&timeout_vt, chTimeMS2I(INTER_PACKET_INITIAL_MS), trigger_timeout_event, nullptr); chEvtSignalI(_irq_handler_ctx, EVT_TIMEOUT); chSysUnlockFromISR(); } void AP_Radio_cc2500::start_recv_bind(void) { - protocolState = STATE_BIND_TUNING; chan_count = 0; packet_timer = AP_HAL::micros(); chEvtSignal(_irq_handler_ctx, EVT_BIND); @@ -336,25 +440,25 @@ void AP_Radio_cc2500::handle_data_packet(mavlink_channel_t chan, const mavlink_d uint32_t ofs=0; memcpy(&ofs, &m.data[0], 4); Debug(4, "got data96 of len %u from chan %u at offset %u\n", m.len, chan, unsigned(ofs)); - - WITH_SEMAPHORE(sem); - - fwupload.chan = chan; - fwupload.need_ack = false; - fwupload.offset = ofs; - fwupload.length = MIN(m.len-4, 92); - fwupload.acked = 0; - fwupload.sequence++; - if (m.type == 43) { - // sending a tune to play - for development testing - fwupload.fw_type = TELEM_PLAY; - fwupload.length = MIN(m.len, 90); - fwupload.offset = 0; - memcpy(&fwupload.pending_data[0], &m.data[0], fwupload.length); - } else { - // sending a chunk of firmware OTA upload - fwupload.fw_type = TELEM_FW; - memcpy(&fwupload.pending_data[0], &m.data[4], fwupload.length); + if (sem.take_nonblocking()) { + fwupload.chan = chan; + fwupload.need_ack = false; + fwupload.offset = ofs; + fwupload.length = MIN(m.len-4, 92); + fwupload.acked = 0; + fwupload.sequence++; + if (m.type == 43) { + // sending a tune to play - for development testing + fwupload.fw_type = TELEM_PLAY; + fwupload.length = MIN(m.len, 90); + fwupload.offset = 0; + memcpy(&fwupload.pending_data[0], &m.data[0], fwupload.length); + } else { + // sending a chunk of firmware OTA upload + fwupload.fw_type = TELEM_FW; + memcpy(&fwupload.pending_data[0], &m.data[4], fwupload.length); + } + sem.give(); } } @@ -419,13 +523,21 @@ bool AP_Radio_cc2500::handle_SRT_packet(const uint8_t *packet) // only support version 1 so far return false; } - pwm_channels[0] = pkt->chan1 + 1000 + ((pkt->chan_high&0xC0)<<2); - pwm_channels[1] = pkt->chan2 + 1000 + ((pkt->chan_high&0x30)<<4); - pwm_channels[2] = pkt->chan3 + 1000 + ((pkt->chan_high&0x0C)<<6); - pwm_channels[3] = pkt->chan4 + 1000 + ((pkt->chan_high&0x03)<<8); + uint16_t chan_new[CC2500_MAX_PWM_CHANNELS]; + memcpy(chan_new, pwm_channels, sizeof(pwm_channels)); + + chan_new[0] = pkt->chan1 + 1000 + ((pkt->chan_high&0xC0)<<2); + chan_new[1] = pkt->chan2 + 1000 + ((pkt->chan_high&0x30)<<4); + chan_new[2] = pkt->chan3 + 1000 + ((pkt->chan_high&0x0C)<<6); + chan_new[3] = pkt->chan4 + 1000 + ((pkt->chan_high&0x03)<<8); // we map the buttons onto two PWM channels for ease of integration with ArduPilot - pwm_channels[4] = 1000 + (pkt->buttons & 0x7) * 100; - pwm_channels[5] = 1000 + (pkt->buttons >> 3) * 100; + chan_new[4] = 1000 + (pkt->buttons & 0x7) * 100; + chan_new[5] = 1000 + (pkt->buttons >> 3) * 100; + + // cope with mode1/mode2 + map_stick_mode(chan_new); + + memcpy(pwm_channels, chan_new, sizeof(pwm_channels)); uint8_t data = pkt->data; /* @@ -450,31 +562,47 @@ bool AP_Radio_cc2500::handle_SRT_packet(const uint8_t *packet) break; case PKTYPE_TELEM_PPS: tx_pps = data; + if (!have_tx_pps) { + check_double_bind(); + } + have_tx_pps = true; break; case PKTYPE_BL_VERSION: // unused so far for cc2500 break; + case PKTYPE_RXID1: + if (data != t_status.rxid[0]) { + Debug(4, "Double bind1 - disconnecting\n"); + start_recv_bind(); + } + break; + case PKTYPE_RXID2: + if (data != t_status.rxid[1]) { + Debug(4, "Double bind2 - disconnecting\n"); + start_recv_bind(); + } + break; case PKTYPE_FW_ACK: { - // got an fw upload ack - Debug(4, "ack %u seq=%u acked=%u length=%u len=%u\n", - data, fwupload.sequence, unsigned(fwupload.acked), unsigned(fwupload.length), fwupload.len); - if (fwupload.sequence == data && sem.take_nonblocking()) { - fwupload.sequence++; - fwupload.acked += fwupload.len; - if (fwupload.acked == fwupload.length) { - // trigger send of DATA16 ack to client - fwupload.need_ack = true; - } - sem.give(); + // got an fw upload ack + Debug(4, "ack %u seq=%u acked=%u length=%u len=%u\n", + data, fwupload.sequence, unsigned(fwupload.acked), unsigned(fwupload.length), fwupload.len); + if (fwupload.sequence == data && sem.take_nonblocking()) { + fwupload.sequence++; + fwupload.acked += fwupload.len; + if (fwupload.acked == fwupload.length) { + // trigger send of DATA16 ack to client + fwupload.need_ack = true; } + sem.give(); + } break; } } - + if (chan_count < 7) { chan_count = 7; } - + if (pkt->channr != channr) { Debug(2, "channr=%u hop_chan=%u\n", channr, pkt->channr); channr = pkt->channr; @@ -486,6 +614,211 @@ bool AP_Radio_cc2500::handle_SRT_packet(const uint8_t *packet) return true; } +/* + see if we have already assigned a channel + */ +bool AP_Radio_cc2500::have_channel(uint8_t channel, uint8_t count, uint8_t loop) +{ + uint8_t i; + for (i=0; i 14) { + wifi_chan = 9; + } + cc_wifi_mid = wifi_chan_map[wifi_chan-1]; + if (cc_wifi_mid < wifi_separation) { + cc_wifi_low = 0; + } else { + cc_wifi_low = cc_wifi_mid - wifi_separation; + } + if (cc_wifi_mid > 225) { + cc_wifi_high = 255; + } else { + cc_wifi_high = cc_wifi_mid + wifi_separation; + } + + if (channel_spacing < 7) { + channel_spacing += 7; + } + + for (i=0; i= cc_wifi_high) && !have_channel(channel, i, loop)) { + // accept if not in wifi range and not already allocated + break; + } + } while (loop++ < 100); + val=channel; + // channels to avoid from D16 code, not properly understood + if ((val==0x00) || (val==0x5A) || (val==0xDC)) { + val++; + } + bindHopData[i] = val; + } + + if (get_protocol() != AP_Radio::PROTOCOL_CC2500_GFSK) { + // additional loop to fix any close channels + for (i=0; i= cc_wifi_high) && !have_channel(c, i, 0)) { + bindHopData[i] = c; + break; + } + } + } + // if that fails then accept channels within the wifi band + if (have_channel(bindHopData[i], i, 0)) { + uint8_t c; + for (c = 0; clength != sizeof(struct autobind_packet_cc2500)-1 || + pkt->magic1 != 0xC5 || + pkt->magic2 != 0xA2 || + pkt->txid[0] != uint8_t(~pkt->txid_inverse[0]) || + pkt->txid[1] != uint8_t(~pkt->txid_inverse[1])) { + Debug(3, "AB l=%u el=%u m1=%02x m2=%02x %02x:%02x %02x:%02x %02x:%02x\n", + pkt->length, sizeof(struct autobind_packet_cc2500)-1, pkt->magic1, pkt->magic2, + pkt->txid[0], pkt->txid[1], uint8_t(~pkt->txid_inverse[0]), uint8_t(~pkt->txid_inverse[1]), + pkt->crc[0], pkt->crc[1]); + // not a valid autobind packet + return false; + } + for (uint8_t i=0; ipad); i++) { + if (pkt->pad[i] != i+1) { + Debug(3, "AB pad[%u]=%u\n", i, pkt->pad[i]); + return false; + } + } + uint16_t lcrc = calc_crc(packet,sizeof(*pkt)-2); + if ((lcrc>>8)!=pkt->crc[0] || (lcrc&0x00FF)!=pkt->crc[1]) { + Debug(3, "AB bad CRC\n"); + return false; + } + + uint8_t rssi_raw = packet[sizeof(struct autobind_packet_cc2500)]; + uint8_t rssi_dbm = map_RSSI_to_dBm(rssi_raw); + + if (lqi >= 50) { + Debug(3,"autobind bad LQI %u\n", lqi); + return false; + } + + if (rssi_dbm < get_autobind_rssi()) { + Debug(1,"autobind RSSI %u needs %u\n", (unsigned)rssi_dbm, (unsigned)get_autobind_rssi()); + return false; + } + Debug(1,"autobind RSSI %u above %u lqi=%u bofs=%d\n", (unsigned)rssi_dbm, (unsigned)get_autobind_rssi(), lqi, auto_bindOffset); + + bindOffset = auto_bindOffset; + + bindTxId[0] = pkt->txid[0]; + bindTxId[1] = pkt->txid[1]; + + listLength = NUM_CHANNELS; + t_status.wifi_chan = pkt->wifi_chan; + + setup_hopping_table_SRT(); + + Debug(1,"Saved bind data\n"); + save_bind_info(); + + return true; +} + +/* + map a raw RSSI value to a dBm value + */ +uint8_t AP_Radio_cc2500::map_RSSI_to_dBm(uint8_t rssi_raw) +{ + float rssi_dbm; + if (rssi_raw >= 128) { + rssi_dbm = ((((uint16_t)rssi_raw) * 18) >> 5) - 82; + } else { + rssi_dbm = ((((uint16_t)rssi_raw) * 18) >> 5) + 65; + } + return rssi_dbm; +} + // main IRQ handler void AP_Radio_cc2500::irq_handler(void) { @@ -499,7 +832,7 @@ void AP_Radio_cc2500::irq_handler(void) } while (!matched); if (ccLen & 0x80) { - Debug(3,"Fifo overflow %02x\n", ccLen); + Debug(6,"Fifo overflow %02x\n", ccLen); // RX FIFO overflow cc2500.Strobe(CC2500_SFRX); cc2500.Strobe(CC2500_SRX); @@ -513,14 +846,18 @@ void AP_Radio_cc2500::irq_handler(void) // don't process interrupts in FCCTEST mode return; } - - if (ccLen != 32 && ccLen != sizeof(srt_packet)+2) { + + if (ccLen != 32 && + ccLen != sizeof(srt_packet)+2 && + ccLen != sizeof(autobind_packet_cc2500)+2) { cc2500.Strobe(CC2500_SFRX); cc2500.Strobe(CC2500_SRX); - Debug(3, "bad len %u\n", ccLen); + Debug(4, "bad len %u SRT=%u AB=%u\n", ccLen, + sizeof(srt_packet)+2, + sizeof(autobind_packet_cc2500)+2); return; } - + if (get_debug_level() > 6) { Debug(6, "CC2500 IRQ state=%u\n", unsigned(protocolState)); Debug(6,"len=%u\n", ccLen); @@ -536,10 +873,10 @@ void AP_Radio_cc2500::irq_handler(void) } if (!check_crc(ccLen, packet)) { - Debug(3, "bad CRC ccLen=%u\n", ccLen); + Debug(4, "bad CRC ccLen=%u\n", ccLen); return; } - + switch (protocolState) { case STATE_BIND_TUNING: tuneRx(ccLen, packet); @@ -560,7 +897,7 @@ void AP_Radio_cc2500::irq_handler(void) break; case STATE_STARTING: - listLength = 47; + listLength = NUM_CHANNELS; initialiseData(0); protocolState = STATE_SEARCH; chanskip = 1; @@ -569,7 +906,7 @@ void AP_Radio_cc2500::irq_handler(void) case STATE_SEARCH: protocolState = STATE_DATA; - FALLTHROUGH; + // fallthrough case STATE_DATA: { bool ok = false; @@ -577,38 +914,48 @@ void AP_Radio_cc2500::irq_handler(void) ok = handle_D16_packet(packet); } else if (ccLen == sizeof(srt_packet)+2) { ok = handle_SRT_packet(packet); + if (!ok) { + uint8_t Lqi = packet[ccLen - 1] & 0x7F; + ok = handle_autobind_packet(packet, Lqi); + } } if (ok) { // get RSSI value from status byte uint8_t rssi_raw = packet[ccLen-2]; - float rssi_dbm; - if (rssi_raw >= 128) { - rssi_dbm = ((((uint16_t)rssi_raw) * 18) >> 5) - 82; - } else { - rssi_dbm = ((((uint16_t)rssi_raw) * 18) >> 5) + 65; - } + float rssi_dbm = map_RSSI_to_dBm(rssi_raw); rssi_filtered = 0.95 * rssi_filtered + 0.05 * rssi_dbm; t_status.rssi = uint8_t(MAX(rssi_filtered, 1)); - + + if (stats.recv_packets == 0) { + Debug(3,"cc2500: got 1st packet\n"); + } stats.recv_packets++; packet_timer = irq_time_us; - chVTSet(&timeout_vt, chTimeMS2I(10), trigger_timeout_event, nullptr); - + chVTSet(&timeout_vt, chTimeMS2I(INTER_PACKET_INITIAL_MS), trigger_timeout_event, nullptr); + cc2500.Strobe(CC2500_SIDLE); - cc2500.SetPower(get_transmit_power()); - if (ccLen == 32 || get_protocol() == AP_Radio::PROTOCOL_D16) { - send_D16_telemetry(); - } else { - send_SRT_telemetry(); + if (get_telem_enable()) { + cc2500.SetPower(get_transmit_power()); + if (ccLen == 32 || get_protocol() == AP_Radio::PROTOCOL_D16) { + send_D16_telemetry(); + } else { + if (have_tx_pps) { + /* we don't start sending telemetry until we have the tx_pps rate. This allows us + to reliably detect double-bind, where one TX is bound to multiple RX + */ + send_SRT_telemetry(); + } + } + + // now we sleep for enough time for the packet to be + // transmitted. We can safely sleep here as we have a + // dedicated thread for radio processing. + cc2500.unlock_bus(); + hal.scheduler->delay_microseconds(PACKET_SENT_DELAY_US); + cc2500.lock_bus(); } - - // we can safely sleep here as we have a dedicated thread for radio processing. We need to sleep - // for enough time for the packet to be fully transmitted - cc2500.unlock_bus(); - hal.scheduler->delay_microseconds(3500); - cc2500.lock_bus(); - + nextChannel(chanskip); } break; @@ -625,16 +972,39 @@ void AP_Radio_cc2500::irq_handler(void) } } +/* + setup for the 6 possible FCC channel values (3 normal, 3 CW) + */ +void AP_Radio_cc2500::set_fcc_channel(void) +{ + uint8_t chan = MAX_CHANNEL_NUMBER/2; + switch (get_fcc_test()) { + case 1: + case 4: + chan = 0; + break; + case 2: + case 5: + chan = MAX_CHANNEL_NUMBER/2; + break; + case 3: + case 6: + chan = MAX_CHANNEL_NUMBER-1; + break; + } + setChannel(chan); +} + // handle timeout IRQ void AP_Radio_cc2500::irq_timeout(void) { if (get_fcc_test() != 0 && protocolState != STATE_FCCTEST) { protocolState = STATE_FCCTEST; - Debug(1,"Starting FCCTEST %d\n", get_fcc_test()); - setChannel(labs(get_fcc_test()) * 10); - send_D16_telemetry(); + last_fcc_chan = 0; + set_fcc_channel(); + send_SRT_telemetry(); } - + switch (protocolState) { case STATE_BIND_TUNING: { if (bindOffset >= 126) { @@ -643,7 +1013,7 @@ void AP_Radio_cc2500::irq_timeout(void) } bindOffset = -126; } - uint32_t now = AP_HAL::millis(); + uint32_t now = AP_HAL::millis(); if (now - timeTunedMs > 20) { timeTunedMs = now; bindOffset += 5; @@ -655,11 +1025,11 @@ void AP_Radio_cc2500::irq_timeout(void) } break; } - + case STATE_DATA: { uint32_t now = AP_HAL::micros(); - - if (now - packet_timer > 50*sync_time_us) { + + if (now - packet_timer > SEARCH_START_PKTS*INTER_PACKET_MS*1000UL) { Debug(3,"searching %u\n", unsigned(now - packet_timer)); cc2500.Strobe(CC2500_SIDLE); cc2500.Strobe(CC2500_SFRX); @@ -667,29 +1037,65 @@ void AP_Radio_cc2500::irq_timeout(void) cc2500.Strobe(CC2500_SRX); timeouts++; protocolState = STATE_SEARCH; + search_count = 0; } else { + // to keep the timeouts a constant time behind the + // expected time we need to set the timeout to the + // inter-packet delay again now + chVTSet(&timeout_vt, chTimeMS2I(INTER_PACKET_MS), trigger_timeout_event, nullptr); nextChannel(chanskip); - // to keep the timeouts 1ms behind the expected time we - // need to set the timeout to 9ms - chVTSet(&timeout_vt, chTimeMS2I(9), trigger_timeout_event, nullptr); lost++; } break; } - case STATE_SEARCH: - // shift by one channel at a time when searching - nextChannel(1); + case STATE_SEARCH: { + uint32_t now = AP_HAL::millis(); + search_count++; + if (stats.recv_packets == 0 && + get_autobind_time() != 0 && + get_factory_test() == 0 && + (AP_HAL::micros() - packet_timer) > get_autobind_time() * 1000UL*1000UL && + (search_count & 1) == 0) { + // try for an autobind packet every 2nd packet, waiting 3 packet delays + static uint32_t cc; + auto_bindOffset += 5; + if (auto_bindOffset >= 126) { + auto_bindOffset = -126; + } + Debug(4,"ab recv %u boffset=%d", unsigned(cc), int(auto_bindOffset)); + cc++; + cc2500.WriteRegCheck(CC2500_0C_FSCTRL0, (uint8_t)auto_bindOffset); + setChannelRX(AUTOBIND_CHANNEL); + autobind_start_recv_ms = now; + chVTSet(&timeout_vt, chTimeMS2I(60), trigger_timeout_event, nullptr); + } else { + // shift by one channel at a time when searching + if (autobind_start_recv_ms == 0 || now - autobind_start_recv_ms > 50) { + autobind_start_recv_ms = 0; + cc2500.WriteRegCheck(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); + nextChannel(1); + } + } break; - + } + case STATE_FCCTEST: { if (get_fcc_test() == 0) { protocolState = STATE_DATA; Debug(1,"Ending FCCTEST\n"); } - setChannel(labs(get_fcc_test()) * 10); + // send every 9ms + chVTSet(&timeout_vt, chTimeMS2I(INTER_PACKET_MS), trigger_timeout_event, nullptr); cc2500.SetPower(get_transmit_power()); - send_D16_telemetry(); + if (get_fcc_test() < 4 || last_fcc_chan != get_fcc_test()) { + set_fcc_channel(); + send_SRT_telemetry(); + } + if (last_fcc_chan != get_fcc_test() && get_fcc_test() != 0) { + Debug(1,"Starting FCCTEST %u at power %u\n", unsigned(get_fcc_test()), unsigned(get_transmit_power())); + } + last_fcc_chan = get_fcc_test(); break; } @@ -701,12 +1107,12 @@ void AP_Radio_cc2500::irq_timeout(void) void AP_Radio_cc2500::irq_handler_thd(void *arg) { (void)arg; - while(true) { + while (true) { eventmask_t evt = chEvtWaitAny(ALL_EVENTS); radio_singleton->cc2500.lock_bus(); - - switch(evt) { + + switch (evt) { case EVT_IRQ: if (radio_singleton->protocolState == STATE_FCCTEST) { hal.console->printf("IRQ FCC\n"); @@ -722,7 +1128,18 @@ void AP_Radio_cc2500::irq_handler_thd(void *arg) } break; case EVT_BIND: - radio_singleton->initTuneRx(); + // clear the current bind information + radio_singleton->bindTxId[0] = 1; + radio_singleton->bindTxId[1] = 1; + + radio_singleton->setup_hopping_table_SRT(); + + radio_singleton->protocolState = STATE_SEARCH; + radio_singleton->packet_timer = AP_HAL::micros(); + radio_singleton->stats.recv_packets = 0; + radio_singleton->chanskip = 1; + radio_singleton->nextChannel(1); + radio_singleton->save_bind_info(); break; default: break; @@ -741,22 +1158,15 @@ void AP_Radio_cc2500::initTuneRx(void) best_bindOffset = bindOffset; cc2500.WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); //cc2500.WriteReg(CC2500_07_PKTCTRL1, 0x0C); - cc2500.WriteReg(CC2500_18_MCSM0, 0x8); + //cc2500.WriteReg(CC2500_18_MCSM0, 0x8); - cc2500.Strobe(CC2500_SIDLE); - cc2500.WriteReg(CC2500_23_FSCAL3, calData[0][0]); - cc2500.WriteReg(CC2500_24_FSCAL2, calData[0][1]); - cc2500.WriteReg(CC2500_25_FSCAL1, calData[0][2]); - cc2500.WriteReg(CC2500_0A_CHANNR, 0); - cc2500.Strobe(CC2500_SFRX); - cc2500.Strobe(CC2500_SRX); + setChannelRX(0); } void AP_Radio_cc2500::initialiseData(uint8_t adr) { cc2500.WriteRegCheck(CC2500_0C_FSCTRL0, bindOffset); - cc2500.WriteRegCheck(CC2500_18_MCSM0, 0x8); - cc2500.WriteRegCheck(CC2500_09_ADDR, adr ? 0x03 : bindTxId[0]); + //cc2500.WriteRegCheck(CC2500_18_MCSM0, 0x8); //cc2500.WriteRegCheck(CC2500_07_PKTCTRL1, 0x0D); // address check, no broadcast, autoflush, status enable cc2500.WriteRegCheck(CC2500_19_FOCCFG, 0x16); hal.scheduler->delay_microseconds(10*1000); @@ -764,12 +1174,7 @@ void AP_Radio_cc2500::initialiseData(uint8_t adr) void AP_Radio_cc2500::initGetBind(void) { - cc2500.Strobe(CC2500_SIDLE); - cc2500.WriteReg(CC2500_23_FSCAL3, calData[0][0]); - cc2500.WriteReg(CC2500_24_FSCAL2, calData[0][1]); - cc2500.WriteReg(CC2500_25_FSCAL1, calData[0][2]); - cc2500.WriteReg(CC2500_0A_CHANNR, 0); - cc2500.Strobe(CC2500_SFRX); + setChannelRX(0); hal.scheduler->delay_microseconds(20); // waiting flush FIFO cc2500.Strobe(CC2500_SRX); @@ -845,7 +1250,7 @@ bool AP_Radio_cc2500::getBindData(uint8_t ccLen, uint8_t *packet) } } // bind has finished when we have hopping data for all channels - return (listLength == 47 && bind_mask == ((uint64_t(1)<<47)-1)); + return (listLength == NUM_CHANNELS && bind_mask == ((uint64_t(1)<delay_microseconds(730); +} + +void AP_Radio_cc2500::setChannelRX(uint8_t channel) +{ + setChannel(channel); + cc2500.Strobe(CC2500_SFRX); cc2500.Strobe(CC2500_SRX); } void AP_Radio_cc2500::nextChannel(uint8_t skip) { channr = (channr + skip) % listLength; - setChannel(bindHopData[channr]); + setChannelRX(bindHopData[channr]); } void AP_Radio_cc2500::parse_frSkyX(const uint8_t *packet) @@ -880,8 +1293,8 @@ void AP_Radio_cc2500::parse_frSkyX(const uint8_t *packet) c[7] = (uint16_t)((packet[20]<<4)&0xFF0) | (packet[19]>>4); uint8_t j; - for (uint8_t i=0;i<8;i++) { - if(c[i] > 2047) { + for (uint8_t i=0; i<8; i++) { + if (c[i] > 2047) { j = 8; c[i] = c[i] - 2048; } else { @@ -893,7 +1306,7 @@ void AP_Radio_cc2500::parse_frSkyX(const uint8_t *packet) uint16_t word_temp = (((c[i]-64)<<1)/3+860); if ((word_temp > 800) && (word_temp < 2200)) { uint8_t chan = i+j; - if (chan < CC2500_MAX_CHANNELS) { + if (chan < CC2500_MAX_PWM_CHANNELS) { pwm_channels[chan] = word_temp; if (chan >= chan_count) { chan_count = chan+1; @@ -906,7 +1319,7 @@ void AP_Radio_cc2500::parse_frSkyX(const uint8_t *packet) uint16_t AP_Radio_cc2500::calc_crc(const uint8_t *data, uint8_t len) { uint16_t crc = 0; - for(uint8_t i=0; i < len; i++) { + for (uint8_t i=0; i < len; i++) { crc = (crc<<8) ^ (CRCTable[((uint8_t)(crc>>8) ^ *data++) & 0xFF]); } return crc; @@ -914,11 +1327,10 @@ uint16_t AP_Radio_cc2500::calc_crc(const uint8_t *data, uint8_t len) bool AP_Radio_cc2500::check_crc(uint8_t ccLen, uint8_t *packet) { - if (ccLen == sizeof(srt_packet)+2) { - struct srt_packet *pkt = (struct srt_packet *)packet; - // SRT packet - uint16_t lcrc = calc_crc(packet,sizeof(struct srt_packet)-2); - return lcrc == ((pkt->crc[0]<<8) | pkt->crc[1]); + if (ccLen == sizeof(srt_packet)+2 || + ccLen == sizeof(autobind_packet_cc2500)+2) { + // using hardware CRC + return true; } else if (ccLen == 32) { // D16 packet uint16_t lcrc = calc_crc(&packet[3],(ccLen-7)); @@ -935,12 +1347,12 @@ void AP_Radio_cc2500::save_bind_info(void) // access to storage for bind information StorageAccess bind_storage(StorageManager::StorageBindInfo); struct bind_info info; - + info.magic = bind_magic; info.bindTxId[0] = bindTxId[0]; info.bindTxId[1] = bindTxId[1]; info.bindOffset = bindOffset; - info.listLength = listLength; + info.wifi_chan = t_status.wifi_chan; memcpy(info.bindHopData, bindHopData, sizeof(info.bindHopData)); bind_storage.write_block(0, &info, sizeof(info)); } @@ -957,13 +1369,16 @@ bool AP_Radio_cc2500::load_bind_info(void) if (!bind_storage.read_block(&info, 0, sizeof(info)) || info.magic != bind_magic) { return false; } - + bindTxId[0] = info.bindTxId[0]; bindTxId[1] = info.bindTxId[1]; bindOffset = info.bindOffset; - listLength = info.listLength; + listLength = NUM_CHANNELS; + t_status.wifi_chan = info.wifi_chan; memcpy(bindHopData, info.bindHopData, sizeof(bindHopData)); + setup_hopping_table_SRT(); + return true; } @@ -975,7 +1390,7 @@ void AP_Radio_cc2500::send_D16_telemetry(void) uint8_t frame[15]; memset(frame, 0, sizeof(frame)); - + frame[0] = sizeof(frame)-1; frame[1] = bindTxId[0]; frame[2] = bindTxId[1]; @@ -986,16 +1401,16 @@ void AP_Radio_cc2500::send_D16_telemetry(void) frame[4] = uint8_t(hal.analogin->board_voltage() * 10) & 0x7F; } telem_send_rssi = !telem_send_rssi; - + uint16_t lcrc = calc_crc(&frame[3], 10); frame[13] = lcrc>>8; frame[14] = lcrc; cc2500.Strobe(CC2500_SIDLE); cc2500.Strobe(CC2500_SFTX); - if (get_fcc_test() >= 0) { - // in negative FCC test modes we don't write to the FIFO, which gives - // continuous transmission + if (get_fcc_test() <= 3) { + // in CW FCC test modes we don't write to the FIFO, which + // gives continuous transmission cc2500.WriteFifo(frame, sizeof(frame)); } cc2500.Strobe(CC2500_STX); @@ -1030,15 +1445,16 @@ void AP_Radio_cc2500::send_SRT_telemetry(void) pkt.type = fwupload.fw_type; pkt.payload.fw.seq = fwupload.sequence; uint32_t len = fwupload.length>fwupload.acked?fwupload.length - fwupload.acked:0; - pkt.payload.fw.len = len<=8?len:8; + const uint8_t maxlen = sizeof(pkt.payload.fw.data); + pkt.payload.fw.len = len<=maxlen?len:maxlen; pkt.payload.fw.offset = fwupload.offset+fwupload.acked; memcpy(&pkt.payload.fw.data[0], &fwupload.pending_data[fwupload.acked], pkt.payload.fw.len); fwupload.len = pkt.payload.fw.len; Debug(4, "sent fw seq=%u offset=%u len=%u type=%u\n", - pkt.payload.fw.seq, - pkt.payload.fw.offset, - pkt.payload.fw.len, - pkt.type); + pkt.payload.fw.seq, + pkt.payload.fw.offset, + pkt.payload.fw.len, + pkt.type); sem.give(); } else { pkt.type = TELEM_STATUS; @@ -1047,18 +1463,21 @@ void AP_Radio_cc2500::send_SRT_telemetry(void) pkt.txid[0] = bindTxId[0]; pkt.txid[1] = bindTxId[1]; - uint16_t lcrc = calc_crc((const uint8_t *)&pkt, sizeof(pkt)-2); - pkt.crc[0] = lcrc>>8; - pkt.crc[1] = lcrc&0xFF; - cc2500.Strobe(CC2500_SIDLE); cc2500.Strobe(CC2500_SFTX); - if (get_fcc_test() >= 0) { - // in negative FCC test modes we don't write to the FIFO, which gives + if (get_fcc_test() <= 3) { + // in CW FCC test modes we don't write to the FIFO, which gives // continuous transmission cc2500.WriteFifo((const uint8_t *)&pkt, sizeof(pkt)); } cc2500.Strobe(CC2500_STX); + + if (last_wifi_channel != t_status.wifi_chan) { + setup_hopping_table_SRT(); + save_bind_info(); + } + + telem_send_count++; } /* @@ -1078,5 +1497,73 @@ void AP_Radio_cc2500::check_fw_ack(void) } } +/* + support all 4 rc input modes by swapping channels. + */ +void AP_Radio_cc2500::map_stick_mode(uint16_t *channels) +{ + switch (get_stick_mode()) { + case 1: { + // mode1 + uint16_t tmp = channels[1]; + channels[1] = 3000 - channels[2]; + channels[2] = 3000 - tmp; + break; + } + + case 3: { + // mode3 + uint16_t tmp = channels[1]; + channels[1] = 3000 - channels[2]; + channels[2] = 3000 - tmp; + tmp = channels[0]; + channels[0] = channels[3]; + channels[3] = tmp; + break; + } + + case 4: { + // mode4 + uint16_t tmp = channels[0]; + channels[0] = channels[3]; + channels[3] = tmp; + break; + } + + case 2: + default: + // nothing to do, transmitter is natively mode2 + break; + } +} + +/* + check if we are the 2nd RX bound to this TX + */ +void AP_Radio_cc2500::check_double_bind(void) +{ + if (tx_pps <= telem_send_count || + get_autobind_time() == 0) { + return; + } + // the TX has received more telemetry packets in the last second + // than we have ever sent. There must be another RX sending + // telemetry packets. We will reset our mfg_id and go back waiting + // for a new bind packet, hopefully with the right TX + Debug(1,"Double-bind detected\n"); + + // clear the current bind information + radio_singleton->bindTxId[0] = 1; + radio_singleton->bindTxId[1] = 1; + + radio_singleton->setup_hopping_table_SRT(); + + radio_singleton->protocolState = STATE_SEARCH; + radio_singleton->packet_timer = AP_HAL::micros(); + radio_singleton->stats.recv_packets = 0; + radio_singleton->chanskip = 1; + radio_singleton->nextChannel(1); +} + #endif // HAL_RCINPUT_WITH_AP_RADIO #endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS diff --git a/libraries/AP_Radio/AP_Radio_cc2500.h b/libraries/AP_Radio/AP_Radio_cc2500.h index 113ae3b4ab..ce5feb0c1f 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.h +++ b/libraries/AP_Radio/AP_Radio_cc2500.h @@ -15,7 +15,7 @@ #pragma once /* - AP_Radio implementation for CC2500 2.4GHz radio. + AP_Radio implementation for CC2500 2.4GHz radio. With thanks to cleanflight and betaflight projects */ @@ -26,19 +26,19 @@ #include "telem_structure.h" #include "driver_cc2500.h" -#define CC2500_MAX_CHANNELS 16 +#define CC2500_MAX_PWM_CHANNELS 16 class AP_Radio_cc2500 : public AP_Radio_backend { public: AP_Radio_cc2500(AP_Radio &radio); - + // init - initialise radio bool init(void) override; // rest radio bool reset(void) override; - + // send a packet bool send(const uint8_t *pkt, uint16_t len) override; @@ -61,19 +61,24 @@ public: void update(void) override; // get TX fw version - uint32_t get_tx_version(void) override { + uint32_t get_tx_version(void) override + { // pack date into 16 bits for vendor_id in AUTOPILOT_VERSION return (uint16_t(tx_date.firmware_year)<<12) + (uint16_t(tx_date.firmware_month)<<8) + tx_date.firmware_day; } - + // get radio statistics structure const AP_Radio::stats &get_stats(void) override; // set the 2.4GHz wifi channel used by companion computer, so it can be avoided - void set_wifi_channel(uint8_t channel) override { - // t_status.wifi_chan = channel; + void set_wifi_channel(uint8_t channel) override + { + t_status.wifi_chan = channel; } - + + // static probe function for device detection + static bool probe(void); + private: AP_HAL::OwnPtr dev; static AP_Radio_cc2500 *radio_singleton; @@ -92,11 +97,10 @@ private: AP_Radio::stats stats; AP_Radio::stats last_stats; - uint16_t pwm_channels[CC2500_MAX_CHANNELS]; + uint16_t pwm_channels[CC2500_MAX_PWM_CHANNELS]; Radio_CC2500 cc2500; - uint8_t calData[255][3]; uint8_t bindTxId[2]; int8_t bindOffset; uint8_t bindHopData[47]; @@ -107,7 +111,6 @@ private: int8_t fcc_chan; uint32_t packet_timer; static uint32_t irq_time_us; - const uint32_t sync_time_us = 9000; uint8_t chan_count; uint32_t lost; uint32_t timeouts; @@ -118,8 +121,12 @@ private: uint64_t bind_mask; uint8_t best_lqi; int8_t best_bindOffset; + int8_t auto_bindOffset; + uint8_t search_count; + uint8_t last_wifi_channel; uint32_t timeTunedMs; + uint32_t autobind_start_recv_ms; void initTuneRx(void); void initialiseData(uint8_t adr); @@ -128,6 +135,7 @@ private: bool getBindData(uint8_t ccLen, uint8_t *packet); bool check_best_LQI(void); void setChannel(uint8_t channel); + void setChannelRX(uint8_t channel); void nextChannel(uint8_t skip); void parse_frSkyX(const uint8_t *packet); @@ -141,18 +149,18 @@ private: void irq_timeout(void); // bind structure saved to storage - static const uint16_t bind_magic = 0x120a; + static const uint16_t bind_magic = 0x120c; struct PACKED bind_info { uint16_t magic; uint8_t bindTxId[2]; int8_t bindOffset; - uint8_t listLength; + uint8_t wifi_chan; uint8_t bindHopData[47]; }; - + void save_bind_info(void); bool load_bind_info(void); - + enum { STATE_INIT = 0, STATE_BIND, @@ -171,6 +179,7 @@ private: uint8_t reg; uint8_t value; }; + static const config radio_config_GFSK[]; static const config radio_config[]; struct { @@ -191,17 +200,29 @@ private: uint8_t firmware_month; uint8_t firmware_day; } tx_date; - - struct telem_status t_status; + + struct telem_status_cc2500 t_status; uint32_t last_pps_ms; uint8_t tx_rssi; uint8_t tx_pps; + bool have_tx_pps; + uint8_t last_fcc_chan; + uint32_t telem_send_count; bool handle_D16_packet(const uint8_t *packet); bool handle_SRT_packet(const uint8_t *packet); + bool handle_autobind_packet(const uint8_t *packet, uint8_t lqi); + bool have_channel(uint8_t channel, uint8_t count, uint8_t loop); + void setup_hopping_table_SRT(void); + uint8_t map_RSSI_to_dBm(uint8_t rssi_raw); // check sending of fw upload ack void check_fw_ack(void); + void map_stick_mode(uint16_t *channels); + void set_fcc_channel(void); + + // check for double binding + void check_double_bind(void); }; diff --git a/libraries/AP_Radio/AP_Radio_cypress.cpp b/libraries/AP_Radio/AP_Radio_cypress.cpp index 5c3594979d..3f55bf2e66 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.cpp +++ b/libraries/AP_Radio/AP_Radio_cypress.cpp @@ -21,7 +21,6 @@ https://github.com/esden/superbitrf-firmware */ #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -static THD_WORKING_AREA(_irq_handler_wa, 512); #define TIMEOUT_PRIORITY 181 #define EVT_TIMEOUT EVENT_MASK(0) #define EVT_IRQ EVENT_MASK(1) @@ -41,7 +40,7 @@ static THD_WORKING_AREA(_irq_handler_wa, 512); extern const AP_HAL::HAL& hal; -#define Debug(level, fmt, args...) do { if ((level) <= get_debug_level()) { hal.console->printf(fmt, ##args); }} while (0) +#define Debug(level, fmt, args...) do { if ((level) <= get_debug_level()) { gcs().send_text(MAV_SEVERITY_INFO, fmt, ##args); }} while (0) #define LP_FIFO_SIZE 16 // Physical data FIFO lengths in Radio @@ -262,15 +261,16 @@ bool AP_Radio_cypress::init(void) { dev = hal.spi->get_device(CYRF_SPI_DEVICE); #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - if(_irq_handler_ctx != nullptr) { + if (_irq_handler_ctx != nullptr) { AP_HAL::panic("AP_Radio_cypress: double instantiation of irq_handler\n"); } chVTObjectInit(&timeout_vt); - _irq_handler_ctx = chThdCreateStatic(_irq_handler_wa, - sizeof(_irq_handler_wa), - TIMEOUT_PRIORITY, /* Initial priority. */ - irq_handler_thd, /* Thread function. */ - NULL); /* Thread parameter. */ + _irq_handler_ctx = chThdCreateFromHeap(NULL, + THD_WORKING_AREA_SIZE(2048), + "radio_cypress", + TIMEOUT_PRIORITY, + irq_handler_thd, + NULL); #endif load_bind_info(); @@ -335,7 +335,7 @@ void AP_Radio_cypress::update(void) { check_fw_ack(); } - + /* print one second debug info @@ -350,7 +350,7 @@ void AP_Radio_cypress::print_debug_info(void) num_channels(), unsigned(dsm.send_irq_count), unsigned(dsm.send_count), - dsm.pwm_channels[0], dsm.pwm_channels[1], dsm.pwm_channels[2], dsm.pwm_channels[3], + dsm.pwm_channels[0], dsm.pwm_channels[1], dsm.pwm_channels[2], dsm.pwm_channels[3], dsm.pwm_channels[4], dsm.pwm_channels[5], dsm.pwm_channels[6], dsm.pwm_channels[7], dsm.pwm_channels[13]); } @@ -384,7 +384,7 @@ uint8_t AP_Radio_cypress::num_channels(void) dsm.pwm_channels[chan-1] = dsm.tx_pps; dsm.num_channels = MAX(dsm.num_channels, chan); } - + if (now - last_debug_print_ms > 1000) { last_debug_print_ms = now; if (get_debug_level() > 1) { @@ -437,92 +437,92 @@ bool AP_Radio_cypress::send(const uint8_t *pkt, uint16_t len) /* The PN codes */ const uint8_t AP_Radio_cypress::pn_codes[5][9][8] = { -{ /* Row 0 */ - /* Col 0 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8}, - /* Col 1 */ {0x88, 0x17, 0x13, 0x3B, 0x2D, 0xBF, 0x06, 0xD6}, - /* Col 2 */ {0xF1, 0x94, 0x30, 0x21, 0xA1, 0x1C, 0x88, 0xA9}, - /* Col 3 */ {0xD0, 0xD2, 0x8E, 0xBC, 0x82, 0x2F, 0xE3, 0xB4}, - /* Col 4 */ {0x8C, 0xFA, 0x47, 0x9B, 0x83, 0xA5, 0x66, 0xD0}, - /* Col 5 */ {0x07, 0xBD, 0x9F, 0x26, 0xC8, 0x31, 0x0F, 0xB8}, - /* Col 6 */ {0xEF, 0x03, 0x95, 0x89, 0xB4, 0x71, 0x61, 0x9D}, - /* Col 7 */ {0x40, 0xBA, 0x97, 0xD5, 0x86, 0x4F, 0xCC, 0xD1}, - /* Col 8 */ {0xD7, 0xA1, 0x54, 0xB1, 0x5E, 0x89, 0xAE, 0x86} -}, -{ /* Row 1 */ - /* Col 0 */ {0x83, 0xF7, 0xA8, 0x2D, 0x7A, 0x44, 0x64, 0xD3}, - /* Col 1 */ {0x3F, 0x2C, 0x4E, 0xAA, 0x71, 0x48, 0x7A, 0xC9}, - /* Col 2 */ {0x17, 0xFF, 0x9E, 0x21, 0x36, 0x90, 0xC7, 0x82}, - /* Col 3 */ {0xBC, 0x5D, 0x9A, 0x5B, 0xEE, 0x7F, 0x42, 0xEB}, - /* Col 4 */ {0x24, 0xF5, 0xDD, 0xF8, 0x7A, 0x77, 0x74, 0xE7}, - /* Col 5 */ {0x3D, 0x70, 0x7C, 0x94, 0xDC, 0x84, 0xAD, 0x95}, - /* Col 6 */ {0x1E, 0x6A, 0xF0, 0x37, 0x52, 0x7B, 0x11, 0xD4}, - /* Col 7 */ {0x62, 0xF5, 0x2B, 0xAA, 0xFC, 0x33, 0xBF, 0xAF}, - /* Col 8 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97} -}, -{ /* Row 2 */ - /* Col 0 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97}, - /* Col 1 */ {0x8E, 0x4A, 0xD0, 0xA9, 0xA7, 0xFF, 0x20, 0xCA}, - /* Col 2 */ {0x4C, 0x97, 0x9D, 0xBF, 0xB8, 0x3D, 0xB5, 0xBE}, - /* Col 3 */ {0x0C, 0x5D, 0x24, 0x30, 0x9F, 0xCA, 0x6D, 0xBD}, - /* Col 4 */ {0x50, 0x14, 0x33, 0xDE, 0xF1, 0x78, 0x95, 0xAD}, - /* Col 5 */ {0x0C, 0x3C, 0xFA, 0xF9, 0xF0, 0xF2, 0x10, 0xC9}, - /* Col 6 */ {0xF4, 0xDA, 0x06, 0xDB, 0xBF, 0x4E, 0x6F, 0xB3}, - /* Col 7 */ {0x9E, 0x08, 0xD1, 0xAE, 0x59, 0x5E, 0xE8, 0xF0}, - /* Col 8 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E} -}, -{ /* Row 3 */ - /* Col 0 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E}, - /* Col 1 */ {0x80, 0x69, 0x26, 0x80, 0x08, 0xF8, 0x49, 0xE7}, - /* Col 2 */ {0x7D, 0x2D, 0x49, 0x54, 0xD0, 0x80, 0x40, 0xC1}, - /* Col 3 */ {0xB6, 0xF2, 0xE6, 0x1B, 0x80, 0x5A, 0x36, 0xB4}, - /* Col 4 */ {0x42, 0xAE, 0x9C, 0x1C, 0xDA, 0x67, 0x05, 0xF6}, - /* Col 5 */ {0x9B, 0x75, 0xF7, 0xE0, 0x14, 0x8D, 0xB5, 0x80}, - /* Col 6 */ {0xBF, 0x54, 0x98, 0xB9, 0xB7, 0x30, 0x5A, 0x88}, - /* Col 7 */ {0x35, 0xD1, 0xFC, 0x97, 0x23, 0xD4, 0xC9, 0x88}, - /* Col 8 */ {0x88, 0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40} -}, -{ /* Row 4 */ - /* Col 0 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93}, - /* Col 1 */ {0xDC, 0x68, 0x08, 0x99, 0x97, 0xAE, 0xAF, 0x8C}, - /* Col 2 */ {0xC3, 0x0E, 0x01, 0x16, 0x0E, 0x32, 0x06, 0xBA}, - /* Col 3 */ {0xE0, 0x83, 0x01, 0xFA, 0xAB, 0x3E, 0x8F, 0xAC}, - /* Col 4 */ {0x5C, 0xD5, 0x9C, 0xB8, 0x46, 0x9C, 0x7D, 0x84}, - /* Col 5 */ {0xF1, 0xC6, 0xFE, 0x5C, 0x9D, 0xA5, 0x4F, 0xB7}, - /* Col 6 */ {0x58, 0xB5, 0xB3, 0xDD, 0x0E, 0x28, 0xF1, 0xB0}, - /* Col 7 */ {0x5F, 0x30, 0x3B, 0x56, 0x96, 0x45, 0xF4, 0xA1}, - /* Col 8 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8} -}, + { /* Row 0 */ + /* Col 0 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8}, + /* Col 1 */ {0x88, 0x17, 0x13, 0x3B, 0x2D, 0xBF, 0x06, 0xD6}, + /* Col 2 */ {0xF1, 0x94, 0x30, 0x21, 0xA1, 0x1C, 0x88, 0xA9}, + /* Col 3 */ {0xD0, 0xD2, 0x8E, 0xBC, 0x82, 0x2F, 0xE3, 0xB4}, + /* Col 4 */ {0x8C, 0xFA, 0x47, 0x9B, 0x83, 0xA5, 0x66, 0xD0}, + /* Col 5 */ {0x07, 0xBD, 0x9F, 0x26, 0xC8, 0x31, 0x0F, 0xB8}, + /* Col 6 */ {0xEF, 0x03, 0x95, 0x89, 0xB4, 0x71, 0x61, 0x9D}, + /* Col 7 */ {0x40, 0xBA, 0x97, 0xD5, 0x86, 0x4F, 0xCC, 0xD1}, + /* Col 8 */ {0xD7, 0xA1, 0x54, 0xB1, 0x5E, 0x89, 0xAE, 0x86} + }, + { /* Row 1 */ + /* Col 0 */ {0x83, 0xF7, 0xA8, 0x2D, 0x7A, 0x44, 0x64, 0xD3}, + /* Col 1 */ {0x3F, 0x2C, 0x4E, 0xAA, 0x71, 0x48, 0x7A, 0xC9}, + /* Col 2 */ {0x17, 0xFF, 0x9E, 0x21, 0x36, 0x90, 0xC7, 0x82}, + /* Col 3 */ {0xBC, 0x5D, 0x9A, 0x5B, 0xEE, 0x7F, 0x42, 0xEB}, + /* Col 4 */ {0x24, 0xF5, 0xDD, 0xF8, 0x7A, 0x77, 0x74, 0xE7}, + /* Col 5 */ {0x3D, 0x70, 0x7C, 0x94, 0xDC, 0x84, 0xAD, 0x95}, + /* Col 6 */ {0x1E, 0x6A, 0xF0, 0x37, 0x52, 0x7B, 0x11, 0xD4}, + /* Col 7 */ {0x62, 0xF5, 0x2B, 0xAA, 0xFC, 0x33, 0xBF, 0xAF}, + /* Col 8 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97} + }, + { /* Row 2 */ + /* Col 0 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97}, + /* Col 1 */ {0x8E, 0x4A, 0xD0, 0xA9, 0xA7, 0xFF, 0x20, 0xCA}, + /* Col 2 */ {0x4C, 0x97, 0x9D, 0xBF, 0xB8, 0x3D, 0xB5, 0xBE}, + /* Col 3 */ {0x0C, 0x5D, 0x24, 0x30, 0x9F, 0xCA, 0x6D, 0xBD}, + /* Col 4 */ {0x50, 0x14, 0x33, 0xDE, 0xF1, 0x78, 0x95, 0xAD}, + /* Col 5 */ {0x0C, 0x3C, 0xFA, 0xF9, 0xF0, 0xF2, 0x10, 0xC9}, + /* Col 6 */ {0xF4, 0xDA, 0x06, 0xDB, 0xBF, 0x4E, 0x6F, 0xB3}, + /* Col 7 */ {0x9E, 0x08, 0xD1, 0xAE, 0x59, 0x5E, 0xE8, 0xF0}, + /* Col 8 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E} + }, + { /* Row 3 */ + /* Col 0 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E}, + /* Col 1 */ {0x80, 0x69, 0x26, 0x80, 0x08, 0xF8, 0x49, 0xE7}, + /* Col 2 */ {0x7D, 0x2D, 0x49, 0x54, 0xD0, 0x80, 0x40, 0xC1}, + /* Col 3 */ {0xB6, 0xF2, 0xE6, 0x1B, 0x80, 0x5A, 0x36, 0xB4}, + /* Col 4 */ {0x42, 0xAE, 0x9C, 0x1C, 0xDA, 0x67, 0x05, 0xF6}, + /* Col 5 */ {0x9B, 0x75, 0xF7, 0xE0, 0x14, 0x8D, 0xB5, 0x80}, + /* Col 6 */ {0xBF, 0x54, 0x98, 0xB9, 0xB7, 0x30, 0x5A, 0x88}, + /* Col 7 */ {0x35, 0xD1, 0xFC, 0x97, 0x23, 0xD4, 0xC9, 0x88}, + /* Col 8 */ {0x88, 0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40} + }, + { /* Row 4 */ + /* Col 0 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93}, + /* Col 1 */ {0xDC, 0x68, 0x08, 0x99, 0x97, 0xAE, 0xAF, 0x8C}, + /* Col 2 */ {0xC3, 0x0E, 0x01, 0x16, 0x0E, 0x32, 0x06, 0xBA}, + /* Col 3 */ {0xE0, 0x83, 0x01, 0xFA, 0xAB, 0x3E, 0x8F, 0xAC}, + /* Col 4 */ {0x5C, 0xD5, 0x9C, 0xB8, 0x46, 0x9C, 0x7D, 0x84}, + /* Col 5 */ {0xF1, 0xC6, 0xFE, 0x5C, 0x9D, 0xA5, 0x4F, 0xB7}, + /* Col 6 */ {0x58, 0xB5, 0xB3, 0xDD, 0x0E, 0x28, 0xF1, 0xB0}, + /* Col 7 */ {0x5F, 0x30, 0x3B, 0x56, 0x96, 0x45, 0xF4, 0xA1}, + /* Col 8 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8} + }, }; const uint8_t AP_Radio_cypress::pn_bind[] = { 0x98, 0x88, 0x1B, 0xE4, 0x30, 0x79, 0x03, 0x84 }; /*The CYRF initial config, binding config and transfer config */ const AP_Radio_cypress::config AP_Radio_cypress::cyrf_config[] = { - {CYRF_MODE_OVERRIDE, CYRF_RST}, // Reset the device - {CYRF_CLK_EN, CYRF_RXF}, // Enable the clock - {CYRF_AUTO_CAL_TIME, 0x3C}, // From manual, needed for initialization - {CYRF_AUTO_CAL_OFFSET, 0x14}, // From manual, needed for initialization - {CYRF_RX_CFG, CYRF_LNA | CYRF_FAST_TURN_EN}, // Enable low noise amplifier and fast turning - {CYRF_TX_OFFSET_LSB, 0x55}, // From manual, typical configuration - {CYRF_TX_OFFSET_MSB, 0x05}, // From manual, typical configuration - {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END}, // Force in Synth RX mode - {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_SDR | CYRF_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm - {CYRF_DATA64_THOLD, 0x0E}, // From manual, typical configuration - {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX}, // Set in Synth RX mode (again, really needed?) - {CYRF_IO_CFG, CYRF_IRQ_POL}, // IRQ active high + {CYRF_MODE_OVERRIDE, CYRF_RST}, // Reset the device + {CYRF_CLK_EN, CYRF_RXF}, // Enable the clock + {CYRF_AUTO_CAL_TIME, 0x3C}, // From manual, needed for initialization + {CYRF_AUTO_CAL_OFFSET, 0x14}, // From manual, needed for initialization + {CYRF_RX_CFG, CYRF_LNA | CYRF_FAST_TURN_EN}, // Enable low noise amplifier and fast turning + {CYRF_TX_OFFSET_LSB, 0x55}, // From manual, typical configuration + {CYRF_TX_OFFSET_MSB, 0x05}, // From manual, typical configuration + {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END}, // Force in Synth RX mode + {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_SDR | CYRF_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm + {CYRF_DATA64_THOLD, 0x0E}, // From manual, typical configuration + {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX}, // Set in Synth RX mode (again, really needed?) + {CYRF_IO_CFG, CYRF_IRQ_POL}, // IRQ active high }; const AP_Radio_cypress::config AP_Radio_cypress::cyrf_bind_config[] = { - {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_SDR | CYRF_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm - {CYRF_FRAMING_CFG, CYRF_SOP_LEN | 0xE}, // Set SOP CODE to 64 chips and SOP Correlator Threshold to 0xE - {CYRF_RX_OVERRIDE, CYRF_FRC_RXDR | CYRF_DIS_RXCRC}, // Force receive data rate and disable receive CRC checker - {CYRF_EOP_CTRL, 0x02}, // Only enable EOP symbol count of 2 - {CYRF_TX_OVERRIDE, CYRF_DIS_TXCRC}, // Disable transmit CRC generate + {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_SDR | CYRF_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm + {CYRF_FRAMING_CFG, CYRF_SOP_LEN | 0xE}, // Set SOP CODE to 64 chips and SOP Correlator Threshold to 0xE + {CYRF_RX_OVERRIDE, CYRF_FRC_RXDR | CYRF_DIS_RXCRC}, // Force receive data rate and disable receive CRC checker + {CYRF_EOP_CTRL, 0x02}, // Only enable EOP symbol count of 2 + {CYRF_TX_OVERRIDE, CYRF_DIS_TXCRC}, // Disable transmit CRC generate }; const AP_Radio_cypress::config AP_Radio_cypress::cyrf_transfer_config[] = { - {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_8DR | CYRF_PA_4}, // Enable 64 chip codes, 8DR mode and amplifier +4dBm - {CYRF_FRAMING_CFG, CYRF_SOP_EN | CYRF_SOP_LEN | CYRF_LEN_EN | 0xE}, // Set SOP CODE enable, SOP CODE to 64 chips, Packet length enable, and SOP Correlator Threshold to 0xE - {CYRF_TX_OVERRIDE, 0x00}, // Reset TX overrides - {CYRF_RX_OVERRIDE, 0x00}, // Reset RX overrides + {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_8DR | CYRF_PA_4}, // Enable 64 chip codes, 8DR mode and amplifier +4dBm + {CYRF_FRAMING_CFG, CYRF_SOP_EN | CYRF_SOP_LEN | CYRF_LEN_EN | 0xE}, // Set SOP CODE enable, SOP CODE to 64 chips, Packet length enable, and SOP Correlator Threshold to 0xE + {CYRF_TX_OVERRIDE, 0x00}, // Reset TX overrides + {CYRF_RX_OVERRIDE, 0x00}, // Reset RX overrides }; /* @@ -617,7 +617,7 @@ void AP_Radio_cypress::radio_init(void) if (get_disable_crc()) { write_register(CYRF_RX_OVERRIDE, CYRF_DIS_RXCRC); } - + dsm_setup_transfer_dsmx(); write_register(CYRF_XTAL_CTRL,0x80); // XOUT=BitSerial @@ -632,9 +632,7 @@ void AP_Radio_cypress::radio_init(void) start_receive(); // setup handler for rising edge of IRQ pin -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS hal.gpio->attach_interrupt(HAL_GPIO_RADIO_IRQ, trigger_irq_radio_event, AP_HAL::GPIO::INTERRUPT_RISING); -#endif } void AP_Radio_cypress::dump_registers(uint8_t n) @@ -714,7 +712,7 @@ void AP_Radio_cypress::map_stick_mode(uint16_t *channels) channels[3] = tmp; break; } - + case 2: default: // nothing to do, transmitter is natively mode2 @@ -751,7 +749,7 @@ bool AP_Radio_cypress::parse_dsm_channels(const uint8_t *data) // default value for channels above 4 is previous value memcpy(&pwm_channels[4], &dsm.pwm_channels[4], (max_channels-4)*sizeof(uint16_t)); - + if (!dsm_decode(AP_HAL::micros64(), data, pwm_channels, @@ -772,7 +770,7 @@ bool AP_Radio_cypress::parse_dsm_channels(const uint8_t *data) memcpy(dsm.pwm_channels, pwm_channels, num_values*sizeof(uint16_t)); dsm.last_parse_us = AP_HAL::micros(); - + // suppress channel 8 ack values dsm.num_channels = num_values==8?7:num_values; @@ -876,7 +874,7 @@ void AP_Radio_cypress::process_bind(const uint8_t *pkt, uint8_t len) ok = false; } } - + if (ok) { uint8_t mfg_id[4] = {uint8_t(~pkt[0]), uint8_t(~pkt[1]), uint8_t(~pkt[2]), uint8_t(~pkt[3])}; uint8_t num_chan = pkt[11]; @@ -906,7 +904,7 @@ void AP_Radio_cypress::process_bind(const uint8_t *pkt, uint8_t len) if (is_DSM2()) { dsm2_start_sync(); } - + dsm.need_bind_save = true; } } @@ -932,9 +930,7 @@ void AP_Radio_cypress::dsm2_start_sync(void) */ void AP_Radio_cypress::setup_timeout(uint32_t timeout_ms) { -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS chVTSet(&timeout_vt, chTimeMS2I(timeout_ms), trigger_timeout_event, nullptr); -#endif } /* @@ -946,7 +942,7 @@ void AP_Radio_cypress::process_packet(const uint8_t *pkt, uint8_t len) bool ok; const uint8_t *id = dsm.mfg_id; uint32_t now = AP_HAL::micros(); - + if (is_DSM2()) { ok = (pkt[0] == ((~id[2])&0xFF) && pkt[1] == (~id[3]&0xFF)); } else { @@ -961,7 +957,7 @@ void AP_Radio_cypress::process_packet(const uint8_t *pkt, uint8_t len) } else { if (dsm.current_rf_channel != dsm.channels[0]) { dsm.channels[1] = dsm.current_rf_channel; - dsm.sync = DSM2_OK; + dsm.sync = DSM2_OK; Debug(2, "DSM2 SYNCB chan=%u\n", dsm.channels[1]); dsm.last_recv_us = now; } @@ -991,7 +987,7 @@ void AP_Radio_cypress::process_packet(const uint8_t *pkt, uint8_t len) } else if (packet_dt_us < 8000) { dsm.pkt_time2 = packet_dt_us; } - + if (get_telem_enable()) { if (packet_dt_us < 5000 && (get_autobind_time() == 0 || dsm.have_tx_pps)) { @@ -1014,7 +1010,7 @@ void AP_Radio_cypress::process_packet(const uint8_t *pkt, uint8_t len) stats.bad_packets++; } } else { - stats.bad_packets++; + stats.bad_packets++; } } @@ -1025,7 +1021,7 @@ void AP_Radio_cypress::process_packet(const uint8_t *pkt, uint8_t len) void AP_Radio_cypress::start_receive(void) { dsm_choose_channel(); - + write_register(CYRF_RX_IRQ_STATUS, CYRF_RXOW_IRQ); write_register(CYRF_RX_CTRL, CYRF_RX_GO | CYRF_RXC_IRQEN | CYRF_RXE_IRQEN); @@ -1084,7 +1080,7 @@ void AP_Radio_cypress::irq_handler_recv(uint8_t rx_status) if (state == STATE_AUTOBIND) { state = STATE_RECV; } - + if (state != STATE_SEND_TELEM) { start_receive(); } @@ -1101,7 +1097,7 @@ void AP_Radio_cypress::irq_handler_send(uint8_t tx_status) return; } state = STATE_RECV; - start_receive(); + start_receive(); } @@ -1121,7 +1117,7 @@ void AP_Radio_cypress::irq_handler(void) switch (state) { case STATE_AUTOBIND: - FALLTHROUGH; + // fallthrough case STATE_RECV: case STATE_BIND: irq_handler_recv(rx_status); @@ -1137,7 +1133,7 @@ void AP_Radio_cypress::irq_handler(void) write_register(CYRF_RX_IRQ_STATUS, CYRF_RXOW_IRQ); write_register(CYRF_RX_CTRL, CYRF_RX_GO | CYRF_RXC_IRQEN | CYRF_RXE_IRQEN); break; - + default: break; } @@ -1163,7 +1159,7 @@ void AP_Radio_cypress::irq_timeout(void) Debug(3,"Ending FCC test\n"); state = STATE_RECV; } - + switch (state) { case STATE_SEND_TELEM: send_telem_packet(); @@ -1174,7 +1170,7 @@ void AP_Radio_cypress::irq_timeout(void) case STATE_AUTOBIND: case STATE_SEND_TELEM_WAIT: state = STATE_RECV; - FALLTHROUGH; + // fall through default: write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END); write_register(CYRF_RX_ABORT, 0); @@ -1189,12 +1185,11 @@ void AP_Radio_cypress::irq_timeout(void) /* called on HRT timeout */ -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS void AP_Radio_cypress::irq_handler_thd(void *arg) { _irq_handler_ctx = chThdGetSelfX(); (void)arg; - while(true) { + while (true) { eventmask_t evt = chEvtWaitAny(ALL_EVENTS); if (evt & EVT_IRQ) { radio_singleton->irq_handler(); @@ -1225,7 +1220,7 @@ void AP_Radio_cypress::trigger_irq_radio_event() } chSysUnlockFromISR(); } -#endif + /* Set the current DSM channel with SOP, CRC and data code */ @@ -1263,7 +1258,7 @@ void AP_Radio_cypress::dsm_set_channel(uint8_t channel, bool is_dsm2, uint8_t so Debug(3,"Cypress: TXPOWER=%u\n", dsm.last_transmit_power); write_register(CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_8DR | dsm.last_transmit_power); } - + // Change channel set_channel(channel); } @@ -1280,7 +1275,7 @@ void AP_Radio_cypress::dsm_generate_channels_dsmx(uint8_t mfg_id[4], uint8_t cha uint32_t id_tmp = id; // While not all channels are set - while(idx < 23) { + while (idx < 23) { int i; int count_3_27 = 0, count_28_51 = 0, count_52_76 = 0; @@ -1298,7 +1293,7 @@ void AP_Radio_cypress::dsm_generate_channels_dsmx(uint8_t mfg_id[4], uint8_t cha } // Count the channel groups - if(channels[i] <= 27) { + if (channels[i] <= 27) { count_3_27++; } else if (channels[i] <= 51) { count_28_51++; @@ -1347,7 +1342,7 @@ void AP_Radio_cypress::dsm_choose_channel(void) const uint32_t cycle_time = dsm.pkt_time1 + dsm.pkt_time2; uint8_t next_channel; - + if (state == STATE_BIND) { if (now - dsm.last_chan_change_us > 15000) { // always use odd channel numbers for bind @@ -1369,12 +1364,12 @@ void AP_Radio_cypress::dsm_choose_channel(void) dsm_set_channel(AUTOBIND_CHANNEL, true, 0, 0, 0); state = STATE_AUTOBIND; - + Debug(3,"recv autobind %u\n", unsigned(now - dsm.last_autobind_send)); dsm.last_autobind_send = now; return; } - + if (is_DSM2() && dsm.sync == DSM2_SYNC_A) { if (now - dsm.last_chan_change_us > 15000) { // only even channels for DSM2 scan @@ -1388,7 +1383,7 @@ void AP_Radio_cypress::dsm_choose_channel(void) dsm.sync==DSM2_SYNC_B?~dsm.crc_seed:dsm.crc_seed); return; } - + if (dt < 1000) { // normal channel advance next_channel = dsm.last_recv_chan + 1; @@ -1420,7 +1415,7 @@ void AP_Radio_cypress::dsm_choose_channel(void) dsm.channels[1] = (dsm.channels[1]+2) % DSM_MAX_CHANNEL; } while (dsm.channels[1] == dsm.channels[0]); } - + dsm.current_rf_channel = dsm.channels[dsm.current_channel]; uint16_t seed = dsm.crc_seed; @@ -1433,7 +1428,7 @@ void AP_Radio_cypress::dsm_choose_channel(void) dsm2_start_sync(); } } - + dsm_set_channel(dsm.current_rf_channel, is_DSM2(), dsm.sop_col, dsm.data_col, seed); } @@ -1452,7 +1447,7 @@ void AP_Radio_cypress::start_recv_bind(void) write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END); write_register(CYRF_RX_ABORT, 0); - + state = STATE_BIND; radio_set_config(cyrf_bind_config, ARRAY_SIZE(cyrf_bind_config)); @@ -1502,7 +1497,7 @@ void AP_Radio_cypress::load_bind_info(void) struct bind_info info; uint8_t factory_test = get_factory_test(); - + if (factory_test != 0) { Debug(1, "In factory test %u\n", factory_test); memset(dsm.mfg_id, 0, sizeof(dsm.mfg_id)); @@ -1571,10 +1566,10 @@ void AP_Radio_cypress::send_telem_packet(void) memcpy(&pkt.payload.fw.data[0], &fwupload.pending_data[fwupload.acked], pkt.payload.fw.len); fwupload.len = pkt.payload.fw.len; Debug(4, "sent fw seq=%u offset=%u len=%u type=%u\n", - pkt.payload.fw.seq, - pkt.payload.fw.offset, - pkt.payload.fw.len, - pkt.type); + pkt.payload.fw.seq, + pkt.payload.fw.offset, + pkt.payload.fw.len, + pkt.type); sem.give(); pkt.crc = crc_crc8((const uint8_t *)&pkt.type, 15); } else { @@ -1583,7 +1578,7 @@ void AP_Radio_cypress::send_telem_packet(void) pkt.crc = crc_crc8((const uint8_t *)&pkt.type, 15); dsm.telem_send_count++; } - + write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_TX | CYRF_FRC_END); write_register(CYRF_RX_ABORT, 0); transmit16((uint8_t*)&pkt); @@ -1602,7 +1597,7 @@ void AP_Radio_cypress::send_FCC_test_packet(void) state = STATE_SEND_FCC; uint8_t channel=0; - + switch (get_fcc_test()) { case 0: // switch back to normal operation @@ -1629,7 +1624,7 @@ void AP_Radio_cypress::send_FCC_test_packet(void) if (channel != dsm.forced_channel) { Debug(1,"FCC channel %u\n", channel); dsm.forced_channel = channel; - + radio_set_config(cyrf_config, ARRAY_SIZE(cyrf_config)); radio_set_config(cyrf_transfer_config, ARRAY_SIZE(cyrf_transfer_config)); @@ -1643,7 +1638,7 @@ void AP_Radio_cypress::send_FCC_test_packet(void) write_register(CYRF_PREAMBLE,0x01); write_register(CYRF_PREAMBLE,0x00); write_register(CYRF_PREAMBLE,0x00); - + write_register(CYRF_TX_OVERRIDE, CYRF_FRC_PRE); write_register(CYRF_TX_CTRL, CYRF_TX_GO); @@ -1687,7 +1682,7 @@ void AP_Radio_cypress::handle_data_packet(mavlink_channel_t chan, const mavlink_ memcpy(&fwupload.pending_data[0], &m.data[4], fwupload.length); } sem.give(); - } + } } #endif // HAL_RCINPUT_WITH_AP_RADIO diff --git a/libraries/AP_Radio/AP_Radio_cypress.h b/libraries/AP_Radio/AP_Radio_cypress.h index a08fd86bb7..7aa49f615a 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.h +++ b/libraries/AP_Radio/AP_Radio_cypress.h @@ -14,8 +14,10 @@ */ #pragma once +#if HAL_RCINPUT_WITH_AP_RADIO + /* - AP_Radio implementation for Cypress 2.4GHz radio. + AP_Radio implementation for Cypress 2.4GHz radio. With thanks to the SuperBitRF project See http://wiki.paparazziuav.org/wiki/SuperbitRF @@ -24,22 +26,20 @@ */ #include "AP_Radio_backend.h" -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include "hal.h" -#endif #include "telem_structure.h" class AP_Radio_cypress : public AP_Radio_backend { public: AP_Radio_cypress(AP_Radio &radio); - + // init - initialise radio bool init(void) override; // rest radio bool reset(void) override; - + // send a packet bool send(const uint8_t *pkt, uint16_t len) override; @@ -62,31 +62,33 @@ public: void update(void) override; // get TX fw version - uint32_t get_tx_version(void) override { + uint32_t get_tx_version(void) override + { // pack date into 16 bits for vendor_id in AUTOPILOT_VERSION return (uint16_t(dsm.tx_firmware_year)<<12) + (uint16_t(dsm.tx_firmware_month)<<8) + dsm.tx_firmware_day; } - + // get radio statistics structure const AP_Radio::stats &get_stats(void) override; // set the 2.4GHz wifi channel used by companion computer, so it can be avoided - void set_wifi_channel(uint8_t channel) override { + void set_wifi_channel(uint8_t channel) override + { t_status.wifi_chan = channel; } - + private: AP_HAL::OwnPtr dev; static AP_Radio_cypress *radio_singleton; void radio_init(void); - + void dump_registers(uint8_t n); void force_initial_state(void); void set_channel(uint8_t channel); uint8_t read_status_debounced(uint8_t adr); - + uint8_t read_register(uint8_t reg); void write_register(uint8_t reg, uint8_t value); void write_multiple(uint8_t reg, uint8_t n, const uint8_t *data); @@ -99,7 +101,7 @@ private: STATE_SEND_TELEM_WAIT, STATE_SEND_FCC } state; - + struct config { uint8_t reg; uint8_t value; @@ -110,14 +112,12 @@ private: static const config cyrf_bind_config[]; static const config cyrf_transfer_config[]; -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS virtual_timer_t timeout_vt; static thread_t *_irq_handler_ctx; -#endif void radio_set_config(const struct config *config, uint8_t size); void start_receive(void); - + // main IRQ handler void irq_handler(void); @@ -126,21 +126,19 @@ private: // handle timeout IRQ void irq_timeout(void); - + // trampoline functions to take us from static IRQ function to class functions -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS static void irq_handler_thd(void* arg); static void trigger_irq_radio_event(void); static void trigger_timeout_event(void *arg); -#endif static const uint8_t max_channels = 16; uint32_t last_debug_print_ms; void print_debug_info(void); - + AP_Radio::stats stats; AP_Radio::stats last_stats; @@ -160,7 +158,7 @@ private: // semaphore between ISR and main thread HAL_Semaphore sem; - + // dsm config data and status struct { uint8_t channels[23]; @@ -227,7 +225,7 @@ private: }; struct telem_status t_status; - + // DSM specific functions void dsm_set_channel(uint8_t channel, bool is_dsm2, uint8_t sop_col, uint8_t data_col, uint16_t crc_seed); @@ -242,7 +240,7 @@ private: // map for mode1/mode2 void map_stick_mode(uint16_t *channels); - + // parse DSM channels from a packet bool parse_dsm_channels(const uint8_t *data); @@ -267,7 +265,7 @@ private: void irq_handler_send(uint8_t tx_status); void send_FCC_test_packet(void); - + // check sending of fw upload ack void check_fw_ack(void); @@ -281,3 +279,4 @@ private: void setup_timeout(uint32_t timeout_ms); }; +#endif diff --git a/libraries/AP_Radio/driver_bk2425.cpp b/libraries/AP_Radio/driver_bk2425.cpp new file mode 100644 index 0000000000..5ffb8afc85 --- /dev/null +++ b/libraries/AP_Radio/driver_bk2425.cpp @@ -0,0 +1,698 @@ +// -------------------------------------------------------------------- +// low level driver for the beken radio on SPI +// -------------------------------------------------------------------- + +#include "driver_bk2425.h" + +#if defined(HAL_RCINPUT_WITH_AP_RADIO) && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 + +#include +#include +using namespace ChibiOS; + +//#pragma GCC optimize("O0") + +extern const AP_HAL::HAL& hal; + +// -------------------------------------------------------------------- +// Radio initialisation tables +// -------------------------------------------------------------------- + +#if (TX_SPEED==250) +ITX_SPEED Radio_Beken::gTxSpeed = ITX_250; +#elif (TX_SPEED==100) +ITX_SPEED Radio_Beken::gTxSpeed = ITX_1000; +#elif (TX_SPEED==2000) +ITX_SPEED Radio_Beken::gTxSpeed = ITX_2000; +#endif + + +// -------------------------------------------------------------------- +static const uint8_t Bank1_RegTable[ITX_MAX][IREG_MAX][5]= { + // (TX_SPEED == 250u) // [ITX_250] + { + { BK2425_R1_4, 0xf9,0x96,0x8a,0xdb }, // 0xDB8A96f9ul, // [IREG1_4] REG4 250kbps + { BK2425_R1_5, 0x24,0x06,0x0f,0xb6 }, // 0xB60F0624ul, // [IREG1_5] REG5 250kbps + PLL_SPEED, // [IREG1_12] REG12 + { BK2425_R1_13, 0x36,0xb4,0x80,0x00 }, // 0x36B48000ul, // [IREG1_13] REG13 + { BK2425_R1_4, 0xff,0x96,0x8a,0xdb }, // 0xDB8A96f9ul, // [IREG1_4A] REG4 250kbps + }, + // (TX_SPEED == 1000u) [ITX_1000] + { + { BK2425_R1_4, 0xf9,0x96,0x82,0x1b }, // 0x1B8296f9ul, // [IREG1_4] REG4 1Mbps + { BK2425_R1_5, 0x24,0x06,0x0f,0xa6 }, // 0xA60F0624ul, // [IREG1_5] REG5 1Mbps + PLL_SPEED, // [IREG1_12] REG12 + { BK2425_R1_13, 0x36,0xb4,0x80,0x00 }, // 0x36B48000ul, // [IREG1_13] REG13 + { BK2425_R1_4, 0xff,0x96,0x82,0x1b }, // 0x1B8296f9ul, // [IREG1_4A] REG4 1Mbps + }, + // (TX_SPEED == 2000u) [ITX_2000] + { + { BK2425_R1_4, 0xf9,0x96,0x82,0xdb }, // 0xdb8296f9ul, // [IREG1_4] REG4 2Mbps + { BK2425_R1_5, 0x24,0x06,0x0f,0xb6 }, // 0xb60f0624ul, // [IREG1_5] REG5 2Mbps + PLL_SPEED, // [IREG1_12] REG12 + { BK2425_R1_13, 0x36,0xb4,0x80,0x00 }, // 0x36B48000ul, // [IREG1_13] REG13 + { BK2425_R1_4, 0xff,0x96,0x82,0xdb }, // 0xdb8296f9ul, // [IREG1_4A] REG4 2Mbps + }, + // (TX_SPEED == 0u) // [ITX_CARRIER] + { + { BK2425_R1_4, 0xf9,0x96,0x82,0x21 }, // 0xF9968221ul, // [IREG1_4] REG4 carrier + { BK2425_R1_5, 0x24,0x06,0x0f,0xb6 }, // 0xB60F0624ul, // [IREG1_5] REG5 250kbps + PLL_SPEED, // [IREG1_12] REG12 + { BK2425_R1_13, 0x36,0xb4,0x80,0x00 }, // 0x36B48000ul, // [IREG1_13] REG13 + { BK2425_R1_4, 0xff,0x96,0x82,0x21 }, // 0xDB8A96f9ul, // [IREG1_4A] REG4 250kbps + } +}; + + +// -------------------------------------------------------------------- +static const uint8_t Bank0_Reg6[ITX_MAX][2] = { + {BK_RF_SETUP, 0x27}, // 250kbps (6) 0x27=250kbps + {BK_RF_SETUP, 0x07}, // 1000kbps (6) 0x07=1Mbps, high gain, high txpower + {BK_RF_SETUP, 0x2F}, // 2000kbps (6) 0x2F=2Mbps, high gain, high txpower + {BK_RF_SETUP, 0x37}, // 250kbps (6) 0x10=carrier +}; + +// -------------------------------------------------------------------- +static const uint8_t Bank1_Reg14[]= { + 0x41,0x20,0x08,0x04,0x81,0x20,0xcf,0xF7,0xfe,0xff,0xff +}; + +// -------------------------------------------------------------------- +// Bank0 register initialization value +static const uint8_t Bank0_Reg[][2]= { + +#if 0 + {BK_CONFIG, BK_CONFIG_PWR_UP | BK_CONFIG_PRIM_RX }, // (0) 0x0F=Rx, PowerUp, no crc, all interrupts enabled + {BK_EN_AA, 0x00}, // (1) 0x00=No auto acknowledge packets on all 6 data pipes (0..5) + {BK_EN_RXADDR, 0x02}, // (2) 0x01=1 or 2 out of 6 data pipes enabled (pairing heartbeat and my tx) + {BK_SETUP_AW, 0x03}, // (3) 0x01=3 byte address width +#else + {BK_CONFIG, BK_CONFIG_EN_CRC | BK_CONFIG_CRCO | BK_CONFIG_PWR_UP | BK_CONFIG_PRIM_RX }, // (0) 0x0F=Rx, PowerUp, crc16, all interrupts enabled + {BK_EN_AA, 0x00}, // (1) 0x00=No auto acknowledge packets on all 6 data pipes (0..5) + {BK_EN_RXADDR, 0x03}, // (2) 0x01=1 or 2 out of 6 data pipes enabled (pairing heartbeat and my tx) + {BK_SETUP_AW, 0x03}, // (3) 0x03=5 byte address width +#endif + {BK_SETUP_RETR, 0x00}, // (4) 0x00=No retransmissions + {BK_RF_CH, 0x17}, // (5) 0x17=2423Mhz default frequency + + // Comment in Beken code says that 0x0F or 0x2F=2Mbps; 0x07=1Mbps; 0x27=250Kbps +#if (TX_SPEED == 2000) + {BK_RF_SETUP, 0x2F}, // (6) 0x2F=2Mbps, high gain, high txpower +#elif (TX_SPEED == 1000) + {BK_RF_SETUP, 0x07}, // (6) 0x07=1Mbps, high gain, high txpower +#elif (TX_SPEED == 250) + {BK_RF_SETUP, 0x27}, // (6) 0x27=250kbps + //{BK_RF_SETUP, 0x21}, // (6) 0x27=250kbps, lowest txpower +#endif + + {BK_STATUS, 0x07}, // (7) 7=no effect + {BK_OBSERVE_TX, 0x00}, // (8) (no effect) + {BK_CD, 0x00}, // (9) Carrier detect (no effect) + // (10) = 5 byte register + // (11) = 5 byte register + {BK_RX_ADDR_P2, 0xc3}, // (12) rx address for data pipe 2 + {BK_RX_ADDR_P3, 0xc4}, // (13) rx address for data pipe 3 + {BK_RX_ADDR_P4, 0xc5}, // (14) rx address for data pipe 4 + {BK_RX_ADDR_P5, 0xc6}, // (15) rx address for data pipe 5 + // (16) = 5 byte register + {BK_RX_PW_P0, PACKET_LENGTH_RX_CTRL}, // (17) size of rx data pipe 0 + {BK_RX_PW_P1, PACKET_LENGTH_RX_BIND}, // (18) size of rx data pipe 1 + {BK_RX_PW_P2, 0x20}, // (19) size of rx data pipe 2 + {BK_RX_PW_P3, 0x20}, // (20) size of rx data pipe 3 + {BK_RX_PW_P4, 0x20}, // (21) size of rx data pipe 4 + {BK_RX_PW_P5, 0x20}, // (22) size of rx data pipe 5 + {BK_FIFO_STATUS,0x00}, // (23) fifo status + // (24,25,26,27) + {BK_DYNPD, 0x3F}, // (28) 0x3f=enable dynamic payload length for all 6 data pipes + {BK_FEATURE, BK_FEATURE_EN_DPL | BK_FEATURE_EN_ACK_PAY | BK_FEATURE_EN_DYN_ACK } // (29) 7=enable ack, no ack, dynamic payload length +}; + +// ---------------------------------------------------------------------------- +const uint8_t RegPower[8][2] = { + { OUTPUT_POWER_REG4_0, OUTPUT_POWER_REG6_0 }, + { OUTPUT_POWER_REG4_1, OUTPUT_POWER_REG6_1 }, + { OUTPUT_POWER_REG4_2, OUTPUT_POWER_REG6_2 }, + { OUTPUT_POWER_REG4_3, OUTPUT_POWER_REG6_3 }, + { OUTPUT_POWER_REG4_4, OUTPUT_POWER_REG6_4 }, + { OUTPUT_POWER_REG4_5, OUTPUT_POWER_REG6_5 }, + { OUTPUT_POWER_REG4_6, OUTPUT_POWER_REG6_6 }, + { OUTPUT_POWER_REG4_7, OUTPUT_POWER_REG6_7 }, +}; + +// -------------------------------------------------------------------- +// Generic functions +// -------------------------------------------------------------------- + +// -------------------------------------------------------------------- +// constructor +Radio_Beken::Radio_Beken(AP_HAL::OwnPtr _dev) : + dev(std::move(_dev)) +{ + ResetAddress(); +} + +// -------------------------------------------------------------------- +// Use the default addresses +void Radio_Beken::ResetAddress(void) +{ + // Set the default address + TX_Address[0] = 0x33; + TX_Address[1] = RX0_Address[1] = 0x00; + TX_Address[2] = RX0_Address[2] = 0x59; + TX_Address[3] = RX0_Address[3] = 0x00; + TX_Address[4] = RX0_Address[4] = 0x00; + RX0_Address[0] = 0x31; + RX1_Address[0] = 0x32; + RX1_Address[1] = 0x99; + RX1_Address[2] = 0x59; + RX1_Address[3] = 0xC6; + RX1_Address[4] = 0x2D; +} + +// -------------------------------------------------------------------- +// Raw SPI access functions +// -------------------------------------------------------------------- + +// -------------------------------------------------------------------- +void Radio_Beken::ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t len) +{ + uint8_t tx[len+1]; + uint8_t rx[len+1]; + memset(tx, 0, len+1); + memset(rx, 0, len+1); + tx[0] = address; + DEBUG2_HIGH(); + (void)dev->transfer_fullduplex(tx, rx, len+1); + DEBUG2_LOW(); + memcpy(data, &rx[1], len); +} + +// -------------------------------------------------------------------- +void Radio_Beken::WriteRegisterMulti(uint8_t address, const uint8_t *data, uint8_t len) +{ + uint8_t tx[len+1]; + uint8_t rx[len+1]; + memset(rx, 0, len+1); + tx[0] = address; + memcpy(&tx[1], data, len); + DEBUG2_HIGH(); + (void)dev->transfer_fullduplex(tx, rx, len+1); + DEBUG2_LOW(); +} + +// -------------------------------------------------------------------- +// Low-level Beken functions +// -------------------------------------------------------------------- + +// -------------------------------------------------------------------- +uint8_t Radio_Beken::ReadStatus(void) +{ + uint8_t tx = BK_NOP; + uint8_t rx = 0; + DEBUG2_HIGH(); + (void)dev->transfer_fullduplex(&tx, &rx, 1); + DEBUG2_LOW(); + return rx; // Status +} + +// -------------------------------------------------------------------- +uint8_t Radio_Beken::ReadReg(uint8_t reg) +{ + uint8_t tx[2]; + uint8_t rx[2]; + memset(tx, 0, 2); + memset(rx, 0, 2); + tx[0] = reg | BK_READ_REG; + DEBUG2_HIGH(); + (void)dev->transfer_fullduplex(tx, rx, 2); + DEBUG2_LOW(); + return rx[1]; +} + +// -------------------------------------------------------------------- +uint8_t Radio_Beken::Strobe(uint8_t address) +{ + uint8_t tx = address; + uint8_t rx = 0; + DEBUG2_HIGH(); + (void)dev->transfer_fullduplex(&tx, &rx, 1); + DEBUG2_LOW(); + return rx; // Status +} + +// -------------------------------------------------------------------- +// Set which register bank we are accessing +void Radio_Beken::SetRBank(uint8_t bank) // 1:Bank1 0:Bank0 +{ + uint8_t lastbank = ReadStatus() & BK_STATUS_RBANK; + if (!lastbank != !bank) { + uint8_t tx[2]; + uint8_t rx[2]; + tx[0] = BK_ACTIVATE_CMD; + tx[1] = 0x53; + DEBUG2_HIGH(); + (void)dev->transfer_fullduplex(&tx[0], &rx[0], 2); + DEBUG2_LOW(); + } +} + +// -------------------------------------------------------------------- +void Radio_Beken::WriteReg(uint8_t address, uint8_t data) +{ + uint8_t tx[2]; + uint8_t rx[2]; + memset(rx, 0, 2); + tx[0] = address; // done by caller | BK_WRITE_REG; + tx[1] = data; + DEBUG2_HIGH(); + (void)dev->transfer_fullduplex(tx, rx, 2); + DEBUG2_LOW(); +} + +// -------------------------------------------------------------------- +void Radio_Beken::WriteRegisterMultiBank1(uint8_t address, const uint8_t *data, uint8_t length) +{ + SetRBank(1); + WriteRegisterMulti(address, data, length); + SetRBank(0); +} + +// -------------------------------------------------------------------- +// High-level Beken functions +// -------------------------------------------------------------------- + +// -------------------------------------------------------------------- +// Set the radio transmission power of the beken +// Prerequisite: We should be in idle mode before calling this function +void Radio_Beken::SetPower(uint8_t power) +{ + if (power > 7) { + power = 7; + } + uint8_t oldready = bkReady; + bkReady = 0; + hal.scheduler->delay(100); // delay more than 50ms. + SetRBank(1); + { + const uint8_t* p = &Bank1_RegTable[fcc.CW_mode ? ITX_CARRIER : gTxSpeed][IREG1_4][0]; + uint8_t idx = *p++; + uint8_t buf[4]; + buf[0] = *p++; + buf[1] = *p++; + buf[2] = *p++; + buf[3] = *p++; + buf[0] &= ~0x38; + buf[0] |= (RegPower[power][0] << 3); // Bits 27..29 + WriteRegisterMulti((BK_WRITE_REG|idx), buf, 4); + } + hal.scheduler->delay(100); // delay more than 50ms. + SetRBank(0); + hal.scheduler->delay(100); + + uint8_t setup = ReadReg(BK_RF_SETUP); + setup &= ~(3 << 1); + setup |= (RegPower[power][1] << 1); // Bits 1..2 + if (fcc.CW_mode) { + setup |= 0x10; + } + WriteReg(BK_WRITE_REG|BK_RF_SETUP, setup); + bkReady = oldready; + fcc.power = power; +} + +// -------------------------------------------------------------------- +// Set the physical radio transmission frequency of the beken +void Radio_Beken::SetChannel(uint8_t freq) +{ + lastTxChannel = freq; + WriteReg(BK_WRITE_REG|BK_RF_CH, freq); +} + +// -------------------------------------------------------------------- +// Set the radio transmission mode of the beken +// Enable/disable the carrier sending mode +// Prerequisite: We should be in idle mode before calling this function +void Radio_Beken::SetCwMode(uint8_t cw) +{ + uint8_t oldready = bkReady; + bkReady = 0; + hal.scheduler->delay(100); // delay more than 50ms. + SetRBank(1); + { + const uint8_t* p = &Bank1_RegTable[cw ? ITX_CARRIER : gTxSpeed][IREG1_4][0]; + uint8_t idx = *p++; + uint8_t buf[4]; + buf[0] = *p++; + buf[1] = *p++; + buf[2] = *p++; + buf[3] = *p++; + buf[0] &= ~0x38; + buf[0] |= (RegPower[fcc.power & 7][0] << 3); // Bits 27..29 + WriteRegisterMulti((BK_WRITE_REG|idx), buf, 4); + } + hal.scheduler->delay(100); // delay more than 50ms. + SetRBank(0); + hal.scheduler->delay(100); // delay more than 50ms. + + uint8_t setup = ReadReg(BK_RF_SETUP); + setup &= ~(3 << 1); + setup |= (RegPower[fcc.power & 7][1] << 1); // Bits 1..2 + if (cw) { + setup |= 0x10; + } + WriteReg((BK_WRITE_REG|BK_RF_SETUP), setup); + fcc.CW_mode = cw != 0; + bkReady = oldready; +} + +// -------------------------------------------------------------------- +// Enable/disable the CRC receive mode +// Prerequisite: We should be in idle mode before calling this function +void Radio_Beken::SetCrcMode(uint8_t disable_crc) +{ + uint8_t oldready = bkReady; + bkReady = 0; + + uint8_t config = ReadReg(BK_CONFIG); + if (disable_crc) { + config &= ~(BK_CONFIG_EN_CRC | BK_CONFIG_CRCO); // Disable CRC + } else { + config |= (BK_CONFIG_EN_CRC | BK_CONFIG_CRCO); // Enable CRC + } + WriteReg((BK_WRITE_REG|BK_CONFIG), config); + fcc.disable_crc = (disable_crc != 0); + bkReady = oldready; +} + +// ---------------------------------------------------------------------------- +// Enable the carrier detect feature: Bank1 Reg5 Bit 18 +void Radio_Beken::EnableCarrierDetect(bool bEnable) +{ + if (bEnable == fcc.enable_cd) { + return; + } + uint8_t oldready = bkReady; + bkReady = 0; + SetRBank(1); + { + const uint8_t* p = &Bank1_RegTable[gTxSpeed][IREG1_5][0]; + uint8_t idx = *p++; + uint8_t buf[4]; + buf[0] = *p++; + buf[1] = *p++; + buf[2] = *p++; + buf[3] = *p++; + if (bEnable) { + buf[1] &= ~0x04; + } + WriteRegisterMulti((BK_WRITE_REG|idx), buf, 4); + } + SetRBank(0); + bkReady = oldready; + fcc.enable_cd = bEnable; +} + +// ---------------------------------------------------------------------------- +// Returns true if a carrier is detected +bool Radio_Beken::CarrierDetect(void) +{ + if (fcc.enable_cd) { + if (ReadReg(BK_CD) & 0x01) { + return true; + } + } + return false; +} + +// ---------------------------------------------------------------------------- +void Radio_Beken::SetFactoryMode(uint8_t factory) +{ + uint8_t oldready = bkReady; + bkReady = 0; + + // Set receive/transmit addresses + if (factory) { + // For factory modes, use fixed addresses + TX_Address[0] = 0x35; + TX_Address[1] = RX1_Address[1] = RX0_Address[1] = 0x99; + TX_Address[2] = RX1_Address[2] = RX0_Address[2] = 0x59; + TX_Address[3] = RX1_Address[3] = RX0_Address[3] = 0xC6; + TX_Address[4] = RX1_Address[4] = RX0_Address[4] = factory; + RX0_Address[0] = 0x34; + RX1_Address[0] = 0x43; + } else { + // For normal modes, use the default addresses + ResetAddress(); + } + + // Write the addresses to the registers + WriteRegisterMulti((BK_WRITE_REG|BK_RX_ADDR_P0), RX0_Address, 5); + WriteRegisterMulti((BK_WRITE_REG|BK_RX_ADDR_P1), RX1_Address, 5); + WriteRegisterMulti((BK_WRITE_REG|BK_TX_ADDR), TX_Address, 5); + WriteReg(BK_WRITE_REG|BK_EN_RXADDR, 0x03); + + // Frequency is set by the caller + fcc.factory_mode = factory; + bkReady = oldready; +} + +// ---------------------------------------------------------------------------- +bool Radio_Beken::Reset(void) +{ + //... + hal.scheduler->delay_microseconds(1000); + return 0; +} + +// ---------------------------------------------------------------------------- +// Delay after changing chip-enable +// This can be called from within the interrupt response thread +void Radio_Beken::DelayCE(void) +{ + DEBUG1_LOW(); + hal.scheduler->delay_microseconds(50); + DEBUG1_HIGH(); +} + +// ---------------------------------------------------------------------------- +bool Radio_Beken::WasTxMode(void) +{ + // Were we transmitting something? + return bkMode == BKRADIO_TX; +} + +// ---------------------------------------------------------------------------- +bool Radio_Beken::WasRxMode(void) +{ + // Were we receiving something? + return bkMode == BKRADIO_RX; +} + +// ---------------------------------------------------------------------------- +// Switch to Rx mode +void Radio_Beken::SwitchToRxMode(void) +{ + uint8_t value; + + Strobe(BK_FLUSH_RX); // flush Rx + value = ReadStatus(); // read register STATUS's value + WriteReg(BK_WRITE_REG|BK_STATUS, value); // clear RX_DR or TX_DS or MAX_RT interrupt flag + + BEKEN_CE_LOW(); + DelayCE(); + value = ReadReg(BK_CONFIG); // read register CONFIG's value + value |= BK_CONFIG_PRIM_RX; // set bit 0 + value |= BK_CONFIG_PWR_UP; + WriteReg(BK_WRITE_REG | BK_CONFIG, value); // Set PWR_UP bit, enable CRC(2 length) & Prim:RX. RX_DR enabled.. + + BEKEN_CE_HIGH(); + //BEKEN_PA_LOW(); // we dont have a PA on the RX side + bkMode = BKRADIO_RX; +} + +// ---------------------------------------------------------------------------- +// switch to Tx mode +void Radio_Beken::SwitchToTxMode(void) +{ + uint8_t value; + Strobe(BK_FLUSH_TX); // flush half-sent Tx + Strobe(BK_FLUSH_RX); // flush half-received rx + + // BEKEN_PA_HIGH(); + BEKEN_CE_LOW(); + DelayCE(); + value = ReadReg(BK_CONFIG); // read register CONFIG's value + value &= ~BK_CONFIG_PRIM_RX; // Clear bit 0 (PTX) + value |= BK_CONFIG_PWR_UP; + WriteReg(BK_WRITE_REG | BK_CONFIG, value); // Set PWR_UP bit, enable CRC(2 length) & Prim:RX. RX_DR enabled. + // BEKEN_CE_HIGH(); + bkMode = BKRADIO_TX; +} + +// ---------------------------------------------------------------------------- +// switch to Idle mode +void Radio_Beken::SwitchToIdleMode(void) +{ + Strobe(BK_FLUSH_TX); // flush Tx + Strobe(BK_FLUSH_RX); // flush Rx + + BEKEN_PA_LOW(); + BEKEN_CE_LOW(); + DelayCE(); + bkMode = BKRADIO_IDLE; +} + +// ---------------------------------------------------------------------------- +// Switch to Sleep mode +void Radio_Beken::SwitchToSleepMode(void) +{ + uint8_t value; + Strobe(BK_FLUSH_RX); // flush Rx + Strobe(BK_FLUSH_TX); // flush Tx + value = ReadStatus(); // read register STATUS's value + WriteReg(BK_WRITE_REG|BK_STATUS, value); // clear RX_DR or TX_DS or MAX_RT interrupt flag + + BEKEN_PA_LOW(); + BEKEN_CE_LOW(); + DelayCE(); + value = ReadReg(BK_CONFIG); // read register CONFIG's value + value |= BK_CONFIG_PRIM_RX; // Receive mode + value &= ~BK_CONFIG_PWR_UP; // Power down + WriteReg(BK_WRITE_REG | BK_CONFIG, value); // Clear PWR_UP bit, enable CRC(2 length) & Prim:RX. RX_DR enabled.. + // Stay low + BEKEN_CE_LOW(); + bkMode = BKRADIO_SLEEP; +} + +// ---------------------------------------------------------------------------- +void Radio_Beken::InitBank0Registers(ITX_SPEED spd) +{ + int8_t i; + + //********************Write Bank0 register****************** + for (i=20; i >= 0; i--) { // From BK_FIFO_STATUS back to beginning of table + uint8_t idx = Bank0_Reg[i][0]; + uint8_t value = Bank0_Reg[i][1]; + if (idx == BK_RF_SETUP) { // Adjust for speed + value = Bank0_Reg6[spd][1]; + } + WriteReg((BK_WRITE_REG|idx), value); + } + + // Enable features + i = ReadReg(BK_FEATURE); + if (i == 0) { // i!=0 showed that chip has been actived. So do not active again (as that would toggle these features off again). + WriteReg(BK_ACTIVATE_CMD,0x73); // Activate the BK_FEATURE register. (This command must NOT have BK_WRITE_REG set) + } + for (i = 22; i >= 21; i--) { + WriteReg((BK_WRITE_REG|Bank0_Reg[i][0]),Bank0_Reg[i][1]); + } + + // Set the various 5 byte addresses + WriteRegisterMulti((BK_WRITE_REG|BK_RX_ADDR_P0),RX0_Address,5); // reg 10 - Rx0 addr + WriteRegisterMulti((BK_WRITE_REG|BK_RX_ADDR_P1),RX1_Address,5); // REG 11 - Rx1 addr + WriteRegisterMulti((BK_WRITE_REG|BK_TX_ADDR),TX_Address,5); // REG 16 - TX addr + WriteReg(BK_WRITE_REG|BK_EN_RXADDR, 0x03); + +} + +// ---------------------------------------------------------------------------- +void Radio_Beken::InitBank1Registers(ITX_SPEED spd) +{ + int16_t i; + + for (i = IREG1_4; i <= IREG1_13; i++) { + const uint8_t* p = &Bank1_RegTable[spd][i][0]; + uint8_t idx = *p++; + WriteRegisterMulti((BK_WRITE_REG|idx), p, 4); + } + WriteRegisterMulti((BK_WRITE_REG|BK2425_R1_14),&(Bank1_Reg14[0]),11); + + //toggle REG4<25,26> + { + const uint8_t* p = &Bank1_RegTable[spd][IREG1_4A][0]; + uint8_t idx = *p++; + WriteRegisterMulti((BK_WRITE_REG|idx), p, 4); + } + { + const uint8_t* p = &Bank1_RegTable[spd][IREG1_4][0]; + uint8_t idx = *p++; + WriteRegisterMulti((BK_WRITE_REG|idx), p, 4); + } +} + +// ---------------------------------------------------------------------------- +// Set the rx and tx addresses +void Radio_Beken::SetAddresses(const uint8_t* txaddr) +{ + TX_Address[1] = RX0_Address[1] = txaddr[1]; + TX_Address[3] = RX0_Address[3] = txaddr[3]; + TX_Address[4] = RX0_Address[4] = txaddr[4]; + WriteRegisterMulti((BK_WRITE_REG|BK_RX_ADDR_P0), RX0_Address, 5); + WriteRegisterMulti((BK_WRITE_REG|BK_TX_ADDR), TX_Address, 5); + WriteReg(BK_WRITE_REG|BK_EN_RXADDR, 0x03); +} + +// ---------------------------------------------------------------------------- +bool Radio_Beken::ClearAckOverflow(void) +{ + uint8_t status = ReadStatus(); + if ((BK_STATUS_MAX_RT & status) == 0) { + return false; + } else { + WriteReg((BK_WRITE_REG|BK_STATUS), BK_STATUS_MAX_RT); + return true; + } +} + + +// ---------------------------------------------------------------------------- +// Write a data packet +bool Radio_Beken::SendPacket(uint8_t type, ///< WR_TX_PLOAD or W_TX_PAYLOAD_NOACK_CMD + const uint8_t* pbuf, ///< a buffer pointer + uint8_t len) ///< packet length in bytes +{ + uint8_t fifo_sta = ReadReg(BK_FIFO_STATUS); // read register FIFO_STATUS's value + bool returnValue = ClearAckOverflow(); + + if (!(fifo_sta & BK_FIFO_STATUS_TX_FULL)) { // if not full, send data + numTxPackets++; + WriteRegisterMulti(type, pbuf, len); // Writes data to buffer A0,B0,A8 + BEKEN_CE_HIGH(); // Wait until FIFO has the data before sending it. + } + return returnValue; +} + +// ---------------------------------------------------------------------------- +// For debugging - tell us the current beken register values (from bank 0) +// This just prints it to the UART rather than to the console over WiFi +void Radio_Beken::DumpRegisters(void) +{ + uint8_t i; + for (i = 0; i <= BK_FEATURE; ++i) { + uint8_t len = 1; + switch (i) { + case 10: case 11: case 16: len = 5; break; + case 24: case 25: case 26: case 27: len = 0; break; + default: len = 1; break; + }; + if (len == 1) { + //printf("Bank0reg%d : %x\r\n", i, ReadReg(i)); + } else if (len == 5) { + uint8_t data[5]; + ReadRegisterMulti(i, &data[0], len); + //printf("Bank0reg%d : %x %x %x %x %x\r\n", i, data[0], data[1], data[2], data[3], data[4]); + } + } + SetRBank(1); + for (i = IREG1_4; i <= IREG1_13; ++i) { + uint8_t len = 4; + uint8_t data[4]; + ReadRegisterMulti(i, &data[0], len); + //uint8_t idx = Bank1_RegTable[0][i][0]; + //printf("Bank1reg%d : %x %x %x %x\r\n", idx, data[0], data[1], data[2], data[3]); + } + SetRBank(0); +} + +#endif // HAL_RCINPUT_WITH_AP_RADIO diff --git a/libraries/AP_Radio/driver_bk2425.h b/libraries/AP_Radio/driver_bk2425.h new file mode 100644 index 0000000000..9b43a10b41 --- /dev/null +++ b/libraries/AP_Radio/driver_bk2425.h @@ -0,0 +1,439 @@ +//---------------------------------------------------------------------------------- +// low level driver for the Beken BK2425 radio on SPI +// Note: Under ChiBios the knowledge of which pins are which is not in the driver. +// But ultimately comes from hwdef.dat +//---------------------------------------------------------------------------------- + +#pragma once + +#include + +#if defined(HAL_RCINPUT_WITH_AP_RADIO) && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 + +#define SUPPORT_BK_DEBUG_PINS 0 // 0=UART6 is for GPS, 1=UART6 is debug gpio +#define TX_SPEED 250u // Default transmit speed in kilobits per second. + +/** Channel hopping parameters. Values are in MHz from 2400Mhz. */ +enum CHANNEL_MHZ_e { + CHANNEL_MIN_PHYSICAL = 0, ///< Minimum physical channel that is possible + CHANNEL_MAX_PHYSICAL = 83, ///< Maximum physical channel that is possible + CHANNEL_FCC_LOW = 10, ///< Minimum physical channel that will pass the FCC tests + CHANNEL_FCC_HIGH = 72, ///< Maximum physical channel that will pass the FCC tests + CHANNEL_FCC_MID = 41, ///< A representative physical channel +}; + +enum { + CHANNEL_COUNT_LOGICAL = 16, ///< The maximum number of entries in each frequency table + CHANNEL_BASE_TABLE = 0, ///< The table used for non wifi boards + CHANNEL_SAFE_TABLE = 3, ///< A table that will receive packets even if wrong + CHANNEL_NUM_TABLES = 6, ///< The number of tables + CHANNEL_COUNT_TEST = 16, ///< The number of test mode tables +}; + + +// ---------------------------------------------------------------------------- +// Packet format definition +// ---------------------------------------------------------------------------- + +/** The type of packets being sent between controller and drone */ +enum BK_PKT_TYPE_E { + BK_PKT_TYPE_INVALID = 0, ///< Invalid packet from empty packets or bad CRC + BK_PKT_TYPE_CTRL_FOUND = 0x10, ///< (Tx->Drone) User control - known receiver + BK_PKT_TYPE_CTRL_LOST = 0x11, ///< (Tx->Drone) User control - unknown receiver + BK_PKT_TYPE_BIND_AUTO = 0x12, ///< (Tx->Drone) Tell drones this tx is broadcasting + BK_PKT_TYPE_TELEMETRY = 0x13, ///< (Drone->Tx) Send telemetry to tx + BK_PKT_TYPE_DFU = 0x14, ///< (Drone->Tx) Send new firmware to tx + BK_PKT_TYPE_BIND_MANUAL = 0x15, ///< (Tx->Drone) Tell drones this tx is broadcasting + BK_PKT_TYPE_TUNE = 0x16, ///< (Drone->Tx) Send musical tune to tx +}; +typedef uint8_t BK_PKT_TYPE; + +/** A bitset of the buttons on this controller. */ +enum button_bits { + BUTTON_NONE = 0x00, ///< No buttons are held + BUTTON_RIGHT = 0x01, ///< SW1 = The right button (mode) + BUTTON_LEFT = 0x02, ///< SW2 = The left button (launch/land) + BUTTON_MIDDLE = 0x04, ///< SW3 = The middle button (GPS) + BUTTON_LEFT_SHOULDER = 0x08, ///< SW4 = The left shoulder button (stunt) + BUTTON_RIGHT_SHOULDER = 0x10, ///< SW5 = The right shoulder button (video) + BUTTON_POWER = 0x20, ///< SW6 = The top button (POWER) +}; + +/** The type of info being sent in control packets */ +enum BK_INFO_TYPE_E { + BK_INFO_MIN = 1, + BK_INFO_FW_VER = 1, + BK_INFO_DFU_RX = 2, + BK_INFO_FW_CRC_LO = 3, + BK_INFO_FW_CRC_HI = 4, + BK_INFO_FW_YM = 5, + BK_INFO_FW_DAY = 6, + BK_INFO_MODEL = 7, + BK_INFO_PPS = 8, + BK_INFO_BATTERY = 9, + BK_INFO_COUNTDOWN = 10, + BK_INFO_HOPPING0 = 11, + BK_INFO_HOPPING1 = 12, + BK_INFO_DRONEID0 = 13, + BK_INFO_DRONEID1 = 14, + BK_INFO_MAX +}; +typedef uint8_t BK_INFO_TYPE; + +/** Data for packets that are not droneid packets + Onair order = little-endian */ +typedef struct packetDataDeviceCtrl_s { + uint8_t roll; ///< 2: Low 8 bits of the roll joystick + uint8_t pitch; ///< 3: Low 8 bits of the pitch joystick + uint8_t throttle; ///< 4: Low 8 bits of the throttle joystick + uint8_t yaw; ///< 5: Low 8 bits of the yaw joystick + uint8_t msb; ///< 6: High 2 bits of roll (7..6), pitch (5..4), throttle (3..2), yaw (1..0) + uint8_t buttons_held; ///< 7: The buttons + uint8_t buttons_toggled; ///< 8: The buttons + uint8_t data_type; ///< 9: Type of extra data being sent + uint8_t data_value_lo; ///< 10: Value of extra data being sent + uint8_t data_value_hi; ///< 11: Value of extra data being sent +} packetDataDeviceCtrl; + +enum { SZ_ADDRESS = 5 }; ///< Size of address for transmission packets (40 bits) +enum { SZ_CRC_GUID = 4 }; ///< Size of UUID for drone (32 bits) +enum { SZ_DFU = 16 }; ///< Size of DFU packets + +/** Data for packets that are binding packets + Onair order = little-endian */ +typedef struct packetDataDeviceBind_s { + uint8_t bind_address[SZ_ADDRESS]; ///< The address being used by control packets + uint8_t hopping; ///< The hopping table in use for this connection + uint8_t droneid[SZ_CRC_GUID]; ///< +} packetDataDeviceBind; + +/** Data structure for data packet transmitted from device (controller) to host (drone) */ +typedef struct packetDataDevice_s { + BK_PKT_TYPE packetType; ///< 0: The packet type + uint8_t channel; ///< 1: Next channel I will broadcast on + union packetDataDevice_u { ///< The variant part of the packets + packetDataDeviceCtrl ctrl; ///< Control packets + packetDataDeviceBind bind; ///< Binding packets + } u; +} packetFormatRx; + +/** Data structure for data packet transmitted from host (drone) to device (controller) */ +typedef struct packetDataDrone_s { + BK_PKT_TYPE packetType; ///< 0: The packet type + uint8_t channel; ///< 1: Next channel I will broadcast on + uint8_t pps; ///< 2: Packets per second the drone received + uint8_t flags; ///< 3: Flags + uint8_t droneid[SZ_CRC_GUID]; ///< 4...7: CRC of the drone + uint8_t flight_mode; ///< 8: + uint8_t wifi; ///< 9: Wifi channel + 24 * tx power. + uint8_t note_adjust; ///< 10: note adjust for the tx buzzer (should this be sent so often?) + uint8_t hopping; ///< 11: The adapative hopping byte we want to use +} packetFormatTx; + +typedef struct packetDataDfu_s { + BK_PKT_TYPE packetType; ///< 0: The packet type + uint8_t channel; ///< 1: Next channel I will broadcast on + uint8_t address_lo; ///< 2: + uint8_t address_hi; ///< 3: + uint8_t data[SZ_DFU]; ///< 4...19: +} packetFormatDfu; + + +// ---------------------------------------------------------------------------- +// BK2425 chip definition +// ---------------------------------------------------------------------------- + + +//---------------------------------------------------------------------------------- +/** SPI register commands for the BK2425 and nrf24L01+ chips */ +typedef enum { + // General commands + BK_REG_MASK = 0x1F, // The range of registers that can be read and written + BK_READ_REG = 0x00, // Define read command to register (0..1F) + BK_WRITE_REG = 0x20, // Define write command to register (0..1F) + BK_ACTIVATE_CMD = 0x50, + BK_R_RX_PL_WID_CMD = 0x60, + BK_RD_RX_PLOAD = 0x61, // Define RX payload register address + BK_WR_TX_PLOAD = 0xA0, // Define TX payload register address + BK_W_ACK_PAYLOAD_CMD = 0xA8, // (nrf: +pipe 0..7) + BK_W_TX_PAYLOAD_NOACK_CMD = 0xB0, + BK_FLUSH_TX = 0xE1, // Define flush TX register command + BK_FLUSH_RX = 0xE2, // Define flush RX register command + BK_REUSE_TX_PL = 0xE3, // Define reuse TX payload register command + BK_NOP = 0xFF, // Define No Operation, might be used to read status register + + // BK2425 bank 0 register addresses + BK_CONFIG = 0x00, // 'Config' register address + BK_EN_AA = 0x01, // 'Enable Auto Acknowledgment' register address + BK_EN_RXADDR = 0x02, // 'Enabled RX addresses' register address + BK_SETUP_AW = 0x03, // 'Setup address width' register address + BK_SETUP_RETR = 0x04, // 'Setup Auto. Retrans' register address + BK_RF_CH = 0x05, // 'RF channel' register address + BK_RF_SETUP = 0x06, // 'RF setup' register address + BK_STATUS = 0x07, // 'Status' register address + BK_OBSERVE_TX = 0x08, // 'Observe TX' register address (lost packets, retransmitted packets on this frequency) + BK_CD = 0x09, // 'Carrier Detect' register address + BK_RX_ADDR_P0 = 0x0A, // 'RX address pipe0' register address (5 bytes) + BK_RX_ADDR_P1 = 0x0B, // 'RX address pipe1' register address (5 bytes) + BK_RX_ADDR_P2 = 0x0C, // 'RX address pipe2' register address (1 byte) + BK_RX_ADDR_P3 = 0x0D, // 'RX address pipe3' register address (1 byte) + BK_RX_ADDR_P4 = 0x0E, // 'RX address pipe4' register address (1 byte) + BK_RX_ADDR_P5 = 0x0F, // 'RX address pipe5' register address (1 byte) + BK_TX_ADDR = 0x10, // 'TX address' register address (5 bytes) + BK_RX_PW_P0 = 0x11, // 'RX payload width, pipe0' register address + BK_RX_PW_P1 = 0x12, // 'RX payload width, pipe1' register address + BK_RX_PW_P2 = 0x13, // 'RX payload width, pipe2' register address + BK_RX_PW_P3 = 0x14, // 'RX payload width, pipe3' register address + BK_RX_PW_P4 = 0x15, // 'RX payload width, pipe4' register address + BK_RX_PW_P5 = 0x16, // 'RX payload width, pipe5' register address + BK_FIFO_STATUS = 0x17, // 'FIFO Status Register' register address + BK_DYNPD = 0x1c, // 'Enable dynamic payload length' register address + BK_FEATURE = 0x1d, // 'Feature' register address + BK_PAYLOAD_WIDTH = 0x1f, // 'payload length of 256 bytes modes register address + + // BK2425 bank 1 register addresses + BK2425_R1_4 = 0x04, + BK2425_R1_5 = 0x05, + BK2425_R1_WHOAMI = 0x08, // Register to read that contains the chip id + BK2425_R1_12 = 0x0C, // PLL speed 120 or 130us + BK2425_R1_13 = 0x0D, + BK2425_R1_14 = 0x0E, +} BK_SPI_CMD; + +//---------------------------------------------------------------------------------- +// Chip Status Byte +//---------------------------------------------------------------------------------- + +enum { + BK_CHIP_ID_BK2425 = 0x63, // The expected value of reading BK2425_R1_WHOAMI +}; + +// Meanings of the BK_STATUS register +enum { + BK_STATUS_RBANK = 0x80, // Register bank 1 is in use + BK_STATUS_RX_DR = 0x40, // Data ready + BK_STATUS_TX_DS = 0x20, // Data sent + BK_STATUS_MAX_RT = 0x10, // Max retries failed + BK_STATUS_RX_MASK = 0x0E, // Mask for the receptions bit + BK_STATUS_RX_EMPTY = 0x0E, + BK_STATUS_RX_P_5 = 0x0A, // Data pipe 5 has some data ready + BK_STATUS_RX_P_4 = 0x08, // Data pipe 4 has some data ready + BK_STATUS_RX_P_3 = 0x06, // Data pipe 3 has some data ready + BK_STATUS_RX_P_2 = 0x04, // Data pipe 2 has some data ready + BK_STATUS_RX_P_1 = 0x02, // Data pipe 1 has some data ready + BK_STATUS_RX_P_0 = 0x00, // Data pipe 0 has some data ready + BK_STATUS_TX_FULL = 0x01 // Tx buffer full +}; + +// Meanings of the FIFO_STATUS register +enum { + BK_FIFO_STATUS_TX_REUSE = 0x40, + BK_FIFO_STATUS_TX_FULL = 0x20, + BK_FIFO_STATUS_TX_EMPTY = 0x10, + BK_FIFO_STATUS_RX_FULL = 0x02, + BK_FIFO_STATUS_RX_EMPTY = 0x01 +}; + +// Meanings of the BK_CONFIG register +enum { + BK_CONFIG_MASK_RX_DR = 0x40, // Mask interrupt caused by RX_DR + BK_CONFIG_MASK_TX_DS = 0x20, // Mask interrupt caused by TX_DS + BK_CONFIG_MASK_MAX_RT = 0x10, // Mask interrupt caused by MAX_RT + BK_CONFIG_EN_CRC = 0x08, // Enable CRC. Forced high if one of the bits in the EN_AA is high + BK_CONFIG_CRCO = 0x04, // CRC encoding scheme (0=8 bits, 1=16 bits) + BK_CONFIG_PWR_UP = 0x02, // POWER UP + BK_CONFIG_PRIM_RX = 0x01, // Receive/transmit +}; + +enum { + BK_FEATURE_EN_DPL = 0x04, // + BK_FEATURE_EN_ACK_PAY = 0x02, // + BK_FEATURE_EN_DYN_ACK = 0x01, // +}; + +// (Lets make it one radio interface for both projects) + +/** The baud rate of the GFSK modulation */ +typedef enum ITX_SPEED_e { + ITX_250, ///< 250kbps (slowest but furthest range) + ITX_1000, ///< 1000kbps (balanced) + ITX_2000, ///< 2000kbps (fastest hence least congested) + ITX_CARRIER, ///< 0kbps (constant carrier wave) + ITX_MAX +} ITX_SPEED; + +enum { + PACKET_LENGTH_RX_CTRL = 12, + PACKET_LENGTH_RX_BIND = 12, + PACKET_LENGTH_RX_MAX = 12, + PACKET_LENGTH_TX_TELEMETRY = 12, + PACKET_LENGTH_TX_DFU = 20, + PACKET_LENGTH_TX_MAX = 20, +}; + +// Note that bank 1 registers 0...8 are MSB first; others are LSB first + +#define PLL_SPEED { BK2425_R1_12, 0x00,0x12,0x73,0x05 } // 0x00127305ul, // PLL locking time 130us compatible with nRF24L01; + +// In the array Bank1_Reg0_13[],all the register values are the byte reversed! +enum { + IREG1_4, + IREG1_5, + IREG1_12, + IREG1_13, + IREG1_4A, + IREG_MAX +}; + +#define BK_MAX_PACKET_LEN 32 // max value is 32 bytes +#define BK_RCV_TIMEOUT 30 + +//---------------------------------------------------------------------------------- +// Translate output power into a number +// must match up with the table RegPower[] +#define OUTPUT_POWER_REG6_0 0 // -25dB +#define OUTPUT_POWER_REG6_1 0 // -18dB +#define OUTPUT_POWER_REG6_2 1 // -18dB +#define OUTPUT_POWER_REG6_3 1 // -12dB +#define OUTPUT_POWER_REG6_4 1 // -12dB +#define OUTPUT_POWER_REG6_5 2 // -7dB +#define OUTPUT_POWER_REG6_6 3 // -1dB +#define OUTPUT_POWER_REG6_7 3 // +4dB + +// Register 4 in bank 1 only applies to Beken chip +#define OUTPUT_POWER_REG4_0 0 // -25dB +#define OUTPUT_POWER_REG4_1 3 // -18dB +#define OUTPUT_POWER_REG4_2 0 // -18dB +#define OUTPUT_POWER_REG4_3 3 // -12dB +#define OUTPUT_POWER_REG4_4 2 // -12dB +#define OUTPUT_POWER_REG4_5 0 // -7dB +#define OUTPUT_POWER_REG4_6 0 // -1dB +#define OUTPUT_POWER_REG4_7 7 // +4dB + +// Generic support +#define TOKENPASTE(x, y) x ## y +#define TOKENPASTE2(x, y) TOKENPASTE(x, y) +// The default register values that are for the default power setting +#define DEFAULT_OUTPUT_REG6 TOKENPASTE2(OUTPUT_POWER_REG6_,DEFAULT_OUTPUT_POWER) +#define DEFAULT_OUTPUT_REG4 TOKENPASTE2(OUTPUT_POWER_REG4_,DEFAULT_OUTPUT_POWER) + +// This assumes we are using ChiBios instead of the pixhawk o/s for accessing GPIO +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 +#define BEKEN_SELECT() (dev->set_chip_select(true)) +#define BEKEN_DESELECT() (dev->set_chip_select(false)) +#define BEKEN_CE_HIGH() (palSetLine(HAL_GPIO_PIN_RADIO_CE)) // (hal.gpio->write(HAL_CHIBIOS_GPIO_RADIO_CE, 1)) +#define BEKEN_CE_LOW() (palClearLine(HAL_GPIO_PIN_RADIO_CE)) // (hal.gpio->write(HAL_CHIBIOS_GPIO_RADIO_CE, 0)) +#define BEKEN_PA_HIGH() (palSetLine(HAL_GPIO_PIN_RADIO_PA_CTL)) // (hal.gpio->write(HAL_CHIBIOS_GPIO_RADIO_PA_CTL, 1)) +#define BEKEN_PA_LOW() (palClearLine(HAL_GPIO_PIN_RADIO_PA_CTL)) // (hal.gpio->write(HAL_CHIBIOS_GPIO_RADIO_PA_CTL, 0)) +#if SUPPORT_BK_DEBUG_PINS +#define DEBUG1_HIGH() (palSetLine(HAL_GPIO_PIN_DEBUG1)) +#define DEBUG1_LOW() (palClearLine(HAL_GPIO_PIN_DEBUG1)) +#define DEBUG2_HIGH() (palSetLine(HAL_GPIO_PIN_DEBUG2)) +#define DEBUG2_LOW() (palClearLine(HAL_GPIO_PIN_DEBUG2)) +#else +#define DEBUG1_HIGH() do {} while (0) +#define DEBUG1_LOW() do {} while (0) +#define DEBUG2_HIGH() do {} while (0) +#define DEBUG2_LOW() do {} while (0) +#endif +#else +#error This configuration is not supported. +#endif + +/** Parameters used by the fcc pretests */ +typedef struct FccParams_s { + uint8_t fcc_mode; ///< The value (0..6) last set by the user that we are using. Non-zero iff we are sending test signals + bool scan_mode; ///< true for scanning, false for fixed frequencies + bool CW_mode; ///< true for carrier wave, false for packets + bool disable_crc_mode; ///< false for CRCs enabled, true for CRCs ignored on reception + uint8_t scan_count; ///< In scan mode, packet count before incrementing scan + uint8_t channel; ///< Current frequency 8..70 + uint8_t power; ///< Current power 1..8 + bool disable_crc; ///< true=crc is physically disabled + uint8_t factory_mode; ///< factory test mode 0..8 + bool enable_cd; ///< enable carrier detect + bool last_cd; ///< last carrier detect on a packet received +} FccParams; + +typedef enum BkRadioMode_e { + BKRADIO_SLEEP, + BKRADIO_IDLE, + BKRADIO_TX, + BKRADIO_RX, + BKRADIO_STANDBY1, // Not visible to the code yet + BKRADIO_STANDBY2, // Not visible to the code yet +} BkRadioMode; + +//---------------------------------------------------------------------------------- +// BEKEN driver class +class Radio_Beken +{ +public: + // Generic functions + Radio_Beken(AP_HAL::OwnPtr _dev); + bool lock_bus(void) + { + return dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER); + } + void unlock_bus(void) + { + dev->get_semaphore()->give(); + } + + // Raw SPI access functions + void ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t len); + void WriteRegisterMulti(uint8_t address, const uint8_t *data, uint8_t len); + + // Low-level Beken functions + uint8_t ReadStatus(void); + uint8_t ReadReg(uint8_t reg); + uint8_t Strobe(uint8_t address); + void SetRBank(uint8_t bank); + void WriteReg(uint8_t address, uint8_t data); + void WriteRegisterMultiBank1(uint8_t address, const uint8_t *data, uint8_t len); + + // High-level Beken functions + void SetPower(uint8_t power); + void SetChannel(uint8_t channel); // Sets the physical channel + void SetCwMode(uint8_t cw); + void SetCrcMode(uint8_t disable_crc); // non-zero means crc is ignored + void SetFactoryMode(uint8_t factory); + bool Reset(void); + void SwitchToRxMode(void); + void SwitchToTxMode(void); + void SwitchToIdleMode(void); + void SwitchToSleepMode(void); + void InitBank0Registers(ITX_SPEED spd); + void InitBank1Registers(ITX_SPEED spd); + void SetAddresses(const uint8_t* txaddr); // Set the rx and tx addresses + bool ClearAckOverflow(void); + bool SendPacket(uint8_t type, const uint8_t* pbuf, uint8_t len); + void DelayCE(void); + void DumpRegisters(void); + bool WasTxMode(void); + bool WasRxMode(void); + void ResetAddress(void); + void EnableCarrierDetect(bool bEnable); + bool CarrierDetect(void); + + // Visible public variables (naughty) + volatile uint8_t bkReady; // initialised in AP_Radio_bk2425.h radio_init() at the very end. Similar to a semaphore. + static ITX_SPEED gTxSpeed; + FccParams fcc; + packetFormatTx pktDataTx; // Packet data to send (telemetry) + packetFormatDfu pktDataDfu; // Packet data to send (DFU) + uint8_t TX_Address[5]; // For sending telemetry and DFU + +private: + AP_HAL::OwnPtr dev; + uint8_t bFreshData; // Have we received a packet since we last processed one + uint32_t numTxPackets; + packetFormatRx pktDataRx; // Last valid packet that has been received + packetFormatRx pktDataRecv; // Packet data in process of being received + uint8_t lastTxChannel; // 0..CHANNEL_COUNT_LOGICAL + uint8_t RX0_Address[5]; // The data address + uint8_t RX1_Address[5]; // The fixed binding address + BkRadioMode bkMode; +}; + +#endif diff --git a/libraries/AP_Radio/driver_cc2500.cpp b/libraries/AP_Radio/driver_cc2500.cpp index 82ff595da8..d9e9c3664d 100644 --- a/libraries/AP_Radio/driver_cc2500.cpp +++ b/libraries/AP_Radio/driver_cc2500.cpp @@ -7,7 +7,7 @@ #include "driver_cc2500.h" #include -// #pragma GCC optimize("O0") +#pragma GCC optimize("O0") extern const AP_HAL::HAL& hal; @@ -61,7 +61,7 @@ void Radio_CC2500::WriteReg(uint8_t address, uint8_t data) void Radio_CC2500::SetPower(uint8_t power) { const uint8_t patable[8] = { - 0xC5, // -12dbm + 0xC6, // -12dbm 0x97, // -10dbm 0x6E, // -8dbm 0x7F, // -6dbm @@ -73,7 +73,10 @@ void Radio_CC2500::SetPower(uint8_t power) if (power > 7) { power = 7; } - WriteReg(CC2500_3E_PATABLE, patable[power]); + if (power != last_power) { + WriteReg(CC2500_3E_PATABLE, patable[power]); + last_power = power; + } } bool Radio_CC2500::Reset(void) @@ -95,6 +98,8 @@ void Radio_CC2500::WriteRegCheck(uint8_t address, uint8_t data) while (--tries) { dev->write_register(address, data); uint8_t v = ReadReg(address); - if (v == data) break; + if (v == data) { + break; + } } } diff --git a/libraries/AP_Radio/driver_cc2500.h b/libraries/AP_Radio/driver_cc2500.h index 2038bb2733..ebd38b5832 100644 --- a/libraries/AP_Radio/driver_cc2500.h +++ b/libraries/AP_Radio/driver_cc2500.h @@ -87,21 +87,21 @@ enum { #define CC2500_SRES 0x30 // Reset chip. #define CC2500_SFSTXON \ 0x31 // Enable and calibrate frequency synthesizer (if MCSM0.FS_AUTOCAL=1). - // If in RX/TX: Go to a wait state where only the synthesizer is - // running (for quick RX / TX turnaround). +// If in RX/TX: Go to a wait state where only the synthesizer is +// running (for quick RX / TX turnaround). #define CC2500_SXOFF 0x32 // Turn off crystal oscillator. #define CC2500_SCAL 0x33 // Calibrate frequency synthesizer and turn it off - // (enables quick start). +// (enables quick start). #define CC2500_SRX \ 0x34 // Enable RX. Perform calibration first if coming from IDLE and - // MCSM0.FS_AUTOCAL=1. +// MCSM0.FS_AUTOCAL=1. #define CC2500_STX \ 0x35 // In IDLE state: Enable TX. Perform calibration first if - // MCSM0.FS_AUTOCAL=1. If in RX state and CCA is enabled: - // Only go to TX if channel is clear. +// MCSM0.FS_AUTOCAL=1. If in RX state and CCA is enabled: +// Only go to TX if channel is clear. #define CC2500_SIDLE \ 0x36 // Exit RX / TX, turn off frequency synthesizer and exit - // Wake-On-Radio mode if applicable. +// Wake-On-Radio mode if applicable. #define CC2500_SAFC 0x37 // Perform AFC adjustment of the frequency synthesizer #define CC2500_SWOR 0x38 // Start automatic RX polling sequence (Wake-on-Radio) #define CC2500_SPWD 0x39 // Enter power down mode when CSn goes high. @@ -110,7 +110,7 @@ enum { #define CC2500_SWORRST 0x3C // Reset real time clock. #define CC2500_SNOP \ 0x3D // No operation. May be used to pad strobe commands to two - // bytes for simpler software. +// bytes for simpler software. //---------------------------------------------------------------------------------- // Chip Status Byte //---------------------------------------------------------------------------------- @@ -137,7 +137,8 @@ enum { #define CC2500_LQI_EST_BM 0x7F // CC2500 driver class -class Radio_CC2500 { +class Radio_CC2500 +{ public: Radio_CC2500(AP_HAL::OwnPtr _dev); @@ -154,13 +155,16 @@ public: void SetPower(uint8_t power); bool Reset(void); - bool lock_bus(void) { + bool lock_bus(void) + { return dev && dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER); } - void unlock_bus(void) { + void unlock_bus(void) + { dev->get_semaphore()->give(); } private: AP_HAL::OwnPtr dev; + uint8_t last_power; }; diff --git a/libraries/AP_Radio/telem_structure.h b/libraries/AP_Radio/telem_structure.h index 08139d8b57..f998461be2 100644 --- a/libraries/AP_Radio/telem_structure.h +++ b/libraries/AP_Radio/telem_structure.h @@ -24,7 +24,7 @@ struct PACKED telem_status { uint8_t pps; // packets per second received uint8_t rssi; // lowpass rssi uint8_t flags; // TELEM_FLAG_* - uint8_t flight_mode; // flight mode + uint8_t flight_mode; // flight mode (with profile number in high bit) uint8_t wifi_chan; // wifi channel number on Sonix uint8_t tx_max; // max TX power uint8_t note_adjust; // buzzer tone adjustment @@ -35,7 +35,7 @@ struct PACKED telem_firmware { uint8_t seq; uint8_t len; uint16_t offset; - uint8_t data[8]; + uint8_t data[4]; }; /* @@ -43,12 +43,26 @@ struct PACKED telem_firmware { */ struct PACKED telem_packet_cypress { uint8_t crc; // simple CRC - enum telem_type type; - union { + enum telem_type type; + union { uint8_t pkt[14]; - struct telem_status status; - struct telem_firmware fw; - } payload; + struct telem_status status; + struct telem_firmware fw; + } payload; +}; + +/* + cc2500 specific protocol structures + */ +struct PACKED telem_status_cc2500 { + uint8_t pps; // packets per second received + uint8_t rssi; // lowpass rssi + uint8_t flags; // TELEM_FLAG_* + uint8_t flight_mode; // flight mode (with profile number in high bit) + uint8_t wifi_chan; // wifi channel number on Sonix + uint8_t tx_max; // max TX power + uint8_t note_adjust; // buzzer tone adjustment + uint8_t rxid[2]; // 16 bit ID for cc2500 to prevent double binds }; /* @@ -59,10 +73,23 @@ struct PACKED telem_packet_cc2500 { uint8_t type; uint8_t txid[2]; union { - uint8_t pkt[12]; - struct telem_status status; + uint8_t pkt[9]; + struct telem_status_cc2500 status; struct telem_firmware fw; } payload; +}; + +/* + autobind packet from TX to RX for cc2500 + */ +struct PACKED autobind_packet_cc2500 { + uint8_t length; + uint8_t magic1; // 0xC5 + uint8_t magic2; // 0xA2 + uint8_t txid[2]; + uint8_t txid_inverse[2]; + uint8_t wifi_chan; + uint8_t pad[3]; // pad to 13 bytes for fixed packet length uint8_t crc[2]; }; @@ -79,11 +106,13 @@ enum packet_type { PKTYPE_TELEM_PPS = 5, PKTYPE_BL_VERSION = 6, PKTYPE_FW_ACK = 7, - PKTYPE_NUM_TYPES = 8 // used for modulus + PKTYPE_RXID1 = 8, + PKTYPE_RXID2 = 9, + PKTYPE_NUM_TYPES = 10 // used for modulus }; /* - skyrocket specific packet for cc2500 + skyrocket specific packet from TX to RX for cc2500 */ struct PACKED srt_packet { uint8_t length; // required for cc2500 FIFO @@ -99,5 +128,4 @@ struct PACKED srt_packet { uint8_t buttons; // see channels.h uint8_t channr; uint8_t chanskip; - uint8_t crc[2]; };