From be002665cf6180e28880af81f2745e90c26deee5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 10 Mar 2017 10:37:58 +1100 Subject: [PATCH] AP_Radio: implement CYRF6936 direct attached radio implement DSM2 and DSMX compatible protocols --- libraries/AP_Radio/AP_Radio.cpp | 242 +++ libraries/AP_Radio/AP_Radio.h | 116 ++ libraries/AP_Radio/AP_Radio_backend.cpp | 29 + libraries/AP_Radio/AP_Radio_backend.h | 134 ++ libraries/AP_Radio/AP_Radio_cypress.cpp | 1639 +++++++++++++++++ libraries/AP_Radio/AP_Radio_cypress.h | 278 +++ .../examples/DSMReceiver/DSMReceiver.cpp | 49 + .../AP_Radio/examples/DSMReceiver/wscript | 7 + libraries/AP_Radio/telem_structure.h | 74 + 9 files changed, 2568 insertions(+) create mode 100644 libraries/AP_Radio/AP_Radio.cpp create mode 100644 libraries/AP_Radio/AP_Radio.h create mode 100644 libraries/AP_Radio/AP_Radio_backend.cpp create mode 100644 libraries/AP_Radio/AP_Radio_backend.h create mode 100644 libraries/AP_Radio/AP_Radio_cypress.cpp create mode 100644 libraries/AP_Radio/AP_Radio_cypress.h create mode 100644 libraries/AP_Radio/examples/DSMReceiver/DSMReceiver.cpp create mode 100644 libraries/AP_Radio/examples/DSMReceiver/wscript create mode 100644 libraries/AP_Radio/telem_structure.h diff --git a/libraries/AP_Radio/AP_Radio.cpp b/libraries/AP_Radio/AP_Radio.cpp new file mode 100644 index 0000000000..1485a78fef --- /dev/null +++ b/libraries/AP_Radio/AP_Radio.cpp @@ -0,0 +1,242 @@ +#include + +#ifdef HAL_RCINPUT_WITH_AP_RADIO + +#include "AP_Radio.h" +#include "AP_Radio_backend.h" +#include "AP_Radio_cypress.h" + +extern const AP_HAL::HAL& hal; + +// table of user settable parameters +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 + // @User: Advanced + AP_GROUPINFO("_TYPE", 1, AP_Radio, radio_type, 0), + + // @Param: _PROT + // @DisplayName: protocol + // @Description: Select air protocol + // @Values: 0:Auto,1:DSM2,2:DSMX + // @User: Advanced + AP_GROUPINFO("_PROT", 2, AP_Radio, protocol, PROTOCOL_AUTO), + + // @Param: _DEBUG + // @DisplayName: debug level + // @Description: radio debug level + // @Range: 0 4 + // @User: Advanced + AP_GROUPINFO("_DEBUG", 3, AP_Radio, debug_level, 0), + + // @Param: _DISCRC + // @DisplayName: disable receive CRC + // @Description: disable receive CRC (for debug) + // @Values: 0:NotDisabled,1:Disabled + // @User: Advanced + AP_GROUPINFO("_DISCRC", 4, AP_Radio, disable_crc, 0), + + // @Param: _SIGCH + // @DisplayName: RSSI signal strength + // @Description: Channel to show receive RSSI signal strength, or zero for disabled + // @Range: 0 16 + // @User: Advanced + AP_GROUPINFO("_SIGCH", 5, AP_Radio, rssi_chan, 0), + + // @Param: _PPSCH + // @DisplayName: Packet rate channel + // @Description: Channel to show received packet-per-second rate, or zero for disabled + // @Range: 0 16 + // @User: Advanced + AP_GROUPINFO("_PPSCH", 6, AP_Radio, pps_chan, 0), + + // @Param: _TELEM + // @DisplayName: Enable telemetry + // @Description: If this is non-zero then telemetry packets will be sent over DSM + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("_TELEM", 7, AP_Radio, telem_enable, 0), + + // @Param: _TXPOW + // @DisplayName: Telemetry Transmit power + // @Description: Set telemetry transmit power. This is the power level (from 1 to 8) for telemetry packets sent from the RX to the TX + // @Range: 1 8 + // @User: Advanced + AP_GROUPINFO("_TXPOW", 8, AP_Radio, transmit_power, 8), + + // @Param: _FCCTST + // @DisplayName: Put radio into FCC test mode + // @Description: If this is enabled then the radio will continuously transmit as required for FCC testing. The transmit channel is set by the value of the parameter. The radio will not work for RC input while this is enabled + // @Values: 0:Disabled,1:MinChannel,2:MidChannel,3:MaxChannel,4:MinChannelCW,5:MidChannelCW,6:MaxChannelCW + // @User: Advanced + AP_GROUPINFO("_FCCTST", 9, AP_Radio, fcc_test, 0), + + // @Param: _STKMD + // @DisplayName: Stick input mode + // @Description: This selects between different stick input modes. The default is mode2, which has throttle on the left stick and pitch on the right stick. You can instead set mode1, which has throttle on the right stick and pitch on the left stick. + // @Values: 1:Mode1,2:Mode2 + // @User: Advanced + AP_GROUPINFO("_STKMD", 10, AP_Radio, stick_mode, 2), + + // @Param: _TESTCH + // @DisplayName: Set radio to factory test channel + // @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), + + // @Param: _TSIGCH + // @DisplayName: RSSI value channel for telemetry data on transmitter + // @Description: Channel to show telemetry RSSI value as received by TX + // @Range: 0 16 + // @User: Advanced + AP_GROUPINFO("_TSIGCH", 12, AP_Radio, tx_rssi_chan, 0), + + // @Param: _TPPSCH + // @DisplayName: Telemetry PPS channel + // @Description: Channel to show telemetry packets-per-second value, as received at TX + // @Range: 0 16 + // @User: Advanced + AP_GROUPINFO("_TPPSCH", 13, AP_Radio, tx_pps_chan, 0), + + // @Param: _TXMAX + // @DisplayName: Transmitter transmit power + // @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), + + // @Param: _BZOFS + // @DisplayName: Transmitter buzzer adjustment + // @Description: Set transmitter buzzer note adjustment (adjust frequency up) + // @Range: 0 40 + // @User: Advanced + AP_GROUPINFO("_BZOFS", 15, AP_Radio, tx_buzzer_adjust, 25), + + // @Param: _ABTIME + // @DisplayName: Auto-bind time + // @Description: When non-zero this sets the time with no transmitter packets before we start looking for auto-bind packets. + // @Range: 0 120 + // @User: Advanced + AP_GROUPINFO("_ABTIME", 16, AP_Radio, auto_bind_time, 0), + + // @Param: _ABLVL + // @DisplayName: Auto-bind level + // @Description: This sets the minimum RSSI of an auto-bind packet for it to be accepted. This should be set so that auto-bind will only happen at short range to minimise the change of an auto-bind happening accidentially + // @Range: 0 31 + // @User: Advanced + AP_GROUPINFO("_ABLVL", 17, AP_Radio, auto_bind_rssi, 0), + + AP_GROUPEND +}; + +AP_Radio *AP_Radio::_instance; + +// constructor +AP_Radio::AP_Radio(void) +{ + AP_Param::setup_object_defaults(this, var_info); + if (_instance != nullptr) { + AP_HAL::panic("Multiple AP_Radio declarations"); + } + _instance = this; +} + +bool AP_Radio::init(void) +{ + switch (radio_type) { + case RADIO_TYPE_CYRF6936: + driver = new AP_Radio_cypress(*this); + break; + default: + break; + } + if (!driver) { + return false; + } + return driver->init(); +} + +bool AP_Radio::reset(void) +{ + if (!driver) { + return false; + } + return driver->reset(); +} + +bool AP_Radio::send(const uint8_t *pkt, uint16_t len) +{ + if (!driver) { + return false; + } + return driver->send(pkt, len); +} + +void AP_Radio::start_recv_bind(void) +{ + if (!driver) { + return; + } + return driver->start_recv_bind(); +} + +const AP_Radio::stats &AP_Radio::get_stats(void) +{ + return driver->get_stats(); +} + +uint8_t AP_Radio::num_channels(void) +{ + if (!driver) { + return 0; + } + return driver->num_channels(); +} + +uint16_t AP_Radio::read(uint8_t chan) +{ + if (!driver) { + return 0; + } + return driver->read(chan); +} + +uint32_t AP_Radio::last_recv_us(void) +{ + if (!driver) { + return 0; + } + return driver->last_recv_us(); +} + +// update status, should be called from main thread +void AP_Radio::update(void) +{ + if (driver) { + driver->update(); + } +} + +// get transmitter firmware version +uint32_t AP_Radio::get_tx_version(void) +{ + if (driver) { + return driver->get_tx_version(); + } + return 0; +} + +// set the 2.4GHz wifi channel used by companion computer, so it can be avoided +void AP_Radio::set_wifi_channel(uint8_t channel) +{ + if (driver) { + driver->set_wifi_channel(channel); + } +} + +#endif // HAL_RCINPUT_WITH_AP_RADIO + diff --git a/libraries/AP_Radio/AP_Radio.h b/libraries/AP_Radio/AP_Radio.h new file mode 100644 index 0000000000..47c630dced --- /dev/null +++ b/libraries/AP_Radio/AP_Radio.h @@ -0,0 +1,116 @@ +/* + 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 + +/* + * base class for direct attached radios + */ + +#include +#include +#include + +class AP_Radio_backend; + +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); + + // start bind process as a receiver + void start_recv_bind(void); + + // return time in microseconds of last received R/C packet + uint32_t last_recv_us(void); + + // return number of input channels + uint8_t num_channels(void); + + // return current PWM of a channel + uint16_t read(uint8_t chan); + + // update status, should be called from main thread + void update(void); + + // get transmitter firmware version + uint32_t get_tx_version(void); + + struct stats { + uint32_t bad_packets; + uint32_t recv_errors; + uint32_t recv_packets; + uint32_t lost_packets; + uint32_t timeouts; + }; + + enum ap_radio_type { + RADIO_TYPE_NONE=0, + RADIO_TYPE_CYRF6936=1, + }; + + enum ap_radio_protocol { + PROTOCOL_AUTO=0, + PROTOCOL_DSM2=1, + PROTOCOL_DSMX=2, + }; + + // get packet statistics + const struct stats &get_stats(void); + + static const struct AP_Param::GroupInfo var_info[]; + + // get singleton instance + static AP_Radio *instance(void) { + return _instance; + } + + // set the 2.4GHz wifi channel used by companion computer, so it can be avoided + void set_wifi_channel(uint8_t channel); + +private: + AP_Radio_backend *driver; + + AP_Int8 radio_type; + AP_Int8 protocol; + AP_Int8 debug_level; + AP_Int8 disable_crc; + AP_Int8 rssi_chan; + AP_Int8 pps_chan; + AP_Int8 tx_rssi_chan; + AP_Int8 tx_pps_chan; + AP_Int8 telem_enable; + AP_Int8 transmit_power; + AP_Int8 tx_max_power; + AP_Int8 fcc_test; + AP_Int8 stick_mode; + AP_Int8 factory_test; + AP_Int8 tx_buzzer_adjust; + AP_Int8 auto_bind_time; + AP_Int8 auto_bind_rssi; + + static AP_Radio *_instance; +}; diff --git a/libraries/AP_Radio/AP_Radio_backend.cpp b/libraries/AP_Radio/AP_Radio_backend.cpp new file mode 100644 index 0000000000..b30a509d97 --- /dev/null +++ b/libraries/AP_Radio/AP_Radio_backend.cpp @@ -0,0 +1,29 @@ +/* + 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 . + */ +/* + * backend class for direct attached radios + */ + +#include +#include "AP_Radio_backend.h" + +AP_Radio_backend::AP_Radio_backend(AP_Radio &_radio) : + radio(_radio) +{ +} + +AP_Radio_backend::~AP_Radio_backend(void) +{ +} diff --git a/libraries/AP_Radio/AP_Radio_backend.h b/libraries/AP_Radio/AP_Radio_backend.h new file mode 100644 index 0000000000..e99b0e9350 --- /dev/null +++ b/libraries/AP_Radio/AP_Radio_backend.h @@ -0,0 +1,134 @@ +/* + 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 + +/* + * backend class for direct attached radios + */ + +#include +#include "AP_Radio.h" + +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; + + // start bind process as a receiver + virtual void start_recv_bind(void) = 0; + + // return time in microseconds of last received R/C packet + virtual uint32_t last_recv_us(void) = 0; + + // return number of input channels + virtual uint8_t num_channels(void) = 0; + + // return current PWM of a channel + virtual uint16_t read(uint8_t chan) = 0; + + // update status + virtual void update(void) = 0; + + // 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 { + return (AP_Radio::ap_radio_protocol)radio.protocol.get(); + } + + uint8_t get_debug_level(void) const { + return (uint8_t)radio.debug_level.get(); + } + + bool get_disable_crc(void) const { + return (bool)radio.disable_crc.get(); + } + + uint8_t get_rssi_chan(void) const { + return (uint8_t)radio.rssi_chan.get(); + } + + uint8_t get_pps_chan(void) const { + return (uint8_t)radio.pps_chan.get(); + } + + uint8_t get_tx_rssi_chan(void) const { + return (uint8_t)radio.tx_rssi_chan.get(); + } + + uint8_t get_tx_pps_chan(void) const { + return (uint8_t)radio.tx_pps_chan.get(); + } + + bool get_telem_enable(void) const { + return radio.telem_enable.get() > 0; + } + + uint8_t get_transmit_power(void) const { + return constrain_int16(radio.transmit_power.get(), 1, 8); + } + + 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) { + return radio.tx_max_power.set_default(v); + } + + int8_t get_fcc_test(void) const { + return radio.fcc_test.get(); + } + + uint8_t get_stick_mode(void) const { + return radio.stick_mode.get(); + } + + uint8_t get_factory_test(void) const { + return radio.factory_test.get(); + } + + uint8_t get_tx_buzzer_adjust(void) const { + return radio.tx_buzzer_adjust.get(); + } + + uint8_t get_autobind_time(void) const { + return radio.auto_bind_time.get(); + } + + uint8_t get_autobind_rssi(void) const { + return radio.auto_bind_rssi.get(); + } + + AP_Radio &radio; +}; diff --git a/libraries/AP_Radio/AP_Radio_cypress.cpp b/libraries/AP_Radio/AP_Radio_cypress.cpp new file mode 100644 index 0000000000..161467a9b1 --- /dev/null +++ b/libraries/AP_Radio/AP_Radio_cypress.cpp @@ -0,0 +1,1639 @@ +#include + +#ifdef HAL_RCINPUT_WITH_AP_RADIO + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#include +#endif +#include "AP_Radio_cypress.h" +#include +#include +#include +#include +#include +#include "telem_structure.h" +#include +#include + +/* + driver for CYRF6936 radio + + Many thanks to the SuperBitRF project from Paparrazi for their DSM + configuration code and register defines + https://github.com/esden/superbitrf-firmware + */ + +#ifndef CYRF_IRQ_INPUT +# error "CYRF_IRQ_INPUT must be defined" +#endif + +#ifndef CYRF_RESET_PIN +# define CYRF_RESET_PIN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) +#endif + +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 + +/* The SPI interface defines */ +enum { + CYRF_CHANNEL = 0x00, + CYRF_TX_LENGTH = 0x01, + CYRF_TX_CTRL = 0x02, + CYRF_TX_CFG = 0x03, + CYRF_TX_IRQ_STATUS = 0x04, + CYRF_RX_CTRL = 0x05, + CYRF_RX_CFG = 0x06, + CYRF_RX_IRQ_STATUS = 0x07, + CYRF_RX_STATUS = 0x08, + CYRF_RX_COUNT = 0x09, + CYRF_RX_LENGTH = 0x0A, + CYRF_PWR_CTRL = 0x0B, + CYRF_XTAL_CTRL = 0x0C, + CYRF_IO_CFG = 0x0D, + CYRF_GPIO_CTRL = 0x0E, + CYRF_XACT_CFG = 0x0F, + CYRF_FRAMING_CFG = 0x10, + CYRF_DATA32_THOLD = 0x11, + CYRF_DATA64_THOLD = 0x12, + CYRF_RSSI = 0x13, + CYRF_EOP_CTRL = 0x14, + CYRF_CRC_SEED_LSB = 0x15, + CYRF_CRC_SEED_MSB = 0x16, + CYRF_TX_CRC_LSB = 0x17, + CYRF_TX_CRC_MSB = 0x18, + CYRF_RX_CRC_LSB = 0x19, + CYRF_RX_CRC_MSB = 0x1A, + CYRF_TX_OFFSET_LSB = 0x1B, + CYRF_TX_OFFSET_MSB = 0x1C, + CYRF_MODE_OVERRIDE = 0x1D, + CYRF_RX_OVERRIDE = 0x1E, + CYRF_TX_OVERRIDE = 0x1F, + CYRF_TX_BUFFER = 0x20, + CYRF_RX_BUFFER = 0x21, + CYRF_SOP_CODE = 0x22, + CYRF_DATA_CODE = 0x23, + CYRF_PREAMBLE = 0x24, + CYRF_MFG_ID = 0x25, + CYRF_XTAL_CFG = 0x26, + CYRF_CLK_OFFSET = 0x27, + CYRF_CLK_EN = 0x28, + CYRF_RX_ABORT = 0x29, + CYRF_AUTO_CAL_TIME = 0x32, + CYRF_AUTO_CAL_OFFSET = 0x35, + CYRF_ANALOG_CTRL = 0x39, +}; +#define CYRF_DIR (1<<7) /**< Bit for enabling writing */ + +// CYRF_MODE_OVERRIDE +#define CYRF_RST (1<<0) + +// CYRF_CLK_EN +#define CYRF_RXF (1<<1) + +// CYRF_XACT_CFG +enum { + CYRF_MODE_SLEEP = (0x0<<2), + CYRF_MODE_IDLE = (0x1<<2), + CYRF_MODE_SYNTH_TX = (0x2<<2), + CYRF_MODE_SYNTH_RX = (0x3<<2), + CYRF_MODE_RX = (0x4<<2), +}; +#define CYRF_FRC_END (1<<5) +#define CYRF_ACK_EN (1<<7) + +// CYRF_IO_CFG +#define CYRF_IRQ_GPIO (1<<0) +#define CYRF_SPI_3PIN (1<<1) +#define CYRF_PACTL_GPIO (1<<2) +#define CYRF_PACTL_OD (1<<3) +#define CYRF_XOUT_OD (1<<4) +#define CYRF_MISO_OD (1<<5) +#define CYRF_IRQ_POL (1<<6) +#define CYRF_IRQ_OD (1<<7) + +// CYRF_FRAMING_CFG +#define CYRF_LEN_EN (1<<5) +#define CYRF_SOP_LEN (1<<6) +#define CYRF_SOP_EN (1<<7) + +// CYRF_RX_STATUS +enum { + CYRF_RX_DATA_MODE_GFSK = 0x00, + CYRF_RX_DATA_MODE_8DR = 0x01, + CYRF_RX_DATA_MODE_DDR = 0x10, + CYRF_RX_DATA_MODE_NV = 0x11, +}; +#define CYRF_RX_CODE (1<<2) +#define CYRF_BAD_CRC (1<<3) +#define CYRF_CRC0 (1<<4) +#define CYRF_EOP_ERR (1<<5) +#define CYRF_PKT_ERR (1<<6) +#define CYRF_RX_ACK (1<<7) + +// CYRF_TX_IRQ_STATUS +#define CYRF_TXE_IRQ (1<<0) +#define CYRF_TXC_IRQ (1<<1) +#define CYRF_TXBERR_IRQ (1<<2) +#define CYRF_TXB0_IRQ (1<<3) +#define CYRF_TXB8_IRQ (1<<4) +#define CYRF_TXB15_IRQ (1<<5) +#define CYRF_LV_IRQ (1<<6) +#define CYRF_OS_IRQ (1<<7) + +// CYRF_RX_IRQ_STATUS +#define CYRF_RXE_IRQ (1<<0) +#define CYRF_RXC_IRQ (1<<1) +#define CYRF_RXBERR_IRQ (1<<2) +#define CYRF_RXB1_IRQ (1<<3) +#define CYRF_RXB8_IRQ (1<<4) +#define CYRF_RXB16_IRQ (1<<5) +#define CYRF_SOPDET_IRQ (1<<6) +#define CYRF_RXOW_IRQ (1<<7) + +// CYRF_TX_CTRL +#define CYRF_TXE_IRQEN (1<<0) +#define CYRF_TXC_IRQEN (1<<1) +#define CYRF_TXBERR_IRQEN (1<<2) +#define CYRF_TXB0_IRQEN (1<<3) +#define CYRF_TXB8_IRQEN (1<<4) +#define CYRF_TXB15_IRQEN (1<<5) +#define CYRF_TX_CLR (1<<6) +#define CYRF_TX_GO (1<<7) + +// CYRF_RX_CTRL +#define CYRF_RXE_IRQEN (1<<0) +#define CYRF_RXC_IRQEN (1<<1) +#define CYRF_RXBERR_IRQEN (1<<2) +#define CYRF_RXB1_IRQEN (1<<3) +#define CYRF_RXB8_IRQEN (1<<4) +#define CYRF_RXB16_IRQEN (1<<5) +#define CYRF_RSVD (1<<6) +#define CYRF_RX_GO (1<<7) + +// CYRF_RX_OVERRIDE +#define CYRF_ACE (1<<1) +#define CYRF_DIS_RXCRC (1<<2) +#define CYRF_DIS_CRC0 (1<<3) +#define CYRF_FRC_RXDR (1<<4) +#define CYRF_MAN_RXACK (1<<5) +#define CYRF_RXTX_DLY (1<<6) +#define CYRF_ACK_RX (1<<7) + +// CYRF_TX_OVERRIDE +#define CYRF_TX_INV (1<<0) +#define CYRF_DIS_TXCRC (1<<2) +#define CYRF_OVRD_ACK (1<<3) +#define CYRF_MAN_TXACK (1<<4) +#define CYRF_FRC_PRE (1<<6) +#define CYRF_ACK_TX (1<<7) + +// CYRF_RX_CFG +#define CYRF_VLD_EN (1<<0) +#define CYRF_RXOW_EN (1<<1) +#define CYRF_FAST_TURN_EN (1<<3) +#define CYRF_HILO (1<<4) +#define CYRF_ATT (1<<5) +#define CYRF_LNA (1<<6) +#define CYRF_AGC_EN (1<<7) + +// CYRF_TX_CFG +enum { + CYRF_PA_M35 = 0x0, + CYRF_PA_M30 = 0x1, + CYRF_PA_M24 = 0x2, + CYRF_PA_M18 = 0x3, + CYRF_PA_M13 = 0x4, + CYRF_PA_M5 = 0x5, + CYRF_PA_0 = 0x6, + CYRF_PA_4 = 0x7, +}; +enum { + CYRF_DATA_MODE_GFSK = (0x0 <<3), + CYRF_DATA_MODE_8DR = (0x1 <<3), + CYRF_DATA_MODE_DDR = (0x2 <<3), + CYRF_DATA_MODE_SDR = (0x3 <<3), +}; +#define CYRF_DATA_CODE_LENGTH (1<<5) + + +#define FLAG_WRITE 0x80 +#define FLAG_AUTO_INC 0x40 + +#define DSM_MAX_CHANNEL 0x4F + +#define DSM_SCAN_MIN_CH 8 +#define DSM_SCAN_MID_CH 40 +#define DSM_SCAN_MAX_CH 70 + +#define FCC_SUPPORT_CW_MODE 0 + +#define AUTOBIND_CHANNEL 12 + +// object instance for trampoline +AP_Radio_cypress *AP_Radio_cypress::radio_instance; +/* + constructor + */ +AP_Radio_cypress::AP_Radio_cypress(AP_Radio &_radio) : + AP_Radio_backend(_radio) +{ + // link to instance for irq_trampoline + radio_instance = this; +} + +/* + initialise radio + */ +bool AP_Radio_cypress::init(void) +{ + dev = hal.spi->get_device("cypress"); + load_bind_info(); + + sem = hal.util->new_semaphore(); + + return reset(); +} + +/* + reset radio + */ +bool AP_Radio_cypress::reset(void) +{ + if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return false; + } + + /* + to reset radio hold reset high for 0.5s, then low for 0.5s + */ +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + stm32_configgpio(CYRF_RESET_PIN); + stm32_gpiowrite(CYRF_RESET_PIN, 1); + hal.scheduler->delay(500); + stm32_gpiowrite(CYRF_RESET_PIN, 0); + hal.scheduler->delay(500); + stm32_configgpio(CYRF_IRQ_INPUT); +#endif + radio_init(); + dev->get_semaphore()->give(); + + if (dsm.protocol == DSM_NONE && + get_autobind_time() == 0) { + start_recv_bind(); + } + + return true; +} + +/* + return statistics structure from radio + */ +const AP_Radio::stats &AP_Radio_cypress::get_stats(void) +{ + return stats; +} + +/* + read one pwm channel from radio + */ +uint16_t AP_Radio_cypress::read(uint8_t chan) +{ + if (dsm.need_bind_save) { + save_bind_info(); + } + if (chan >= max_channels) { + return 0; + } + return dsm.pwm_channels[chan]; +} + +/* + update status - called from main thread + */ +void AP_Radio_cypress::update(void) +{ + check_fw_ack(); +} + + +/* + print one second debug info + */ +void AP_Radio_cypress::print_debug_info(void) +{ + Debug(2, "recv:%3u bad:%3u to:%3u re:%u N:%2u TXI:%u TX:%u 1:%4u 2:%4u 3:%4u 4:%4u 5:%4u 6:%4u 7:%4u 8:%4u 14:%u\n", + stats.recv_packets - last_stats.recv_packets, + stats.bad_packets - last_stats.bad_packets, + stats.timeouts - last_stats.timeouts, + stats.recv_errors - last_stats.recv_errors, + num_channels(), + dsm.send_irq_count, + dsm.send_count, + 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]); +} + +/* + return number of active channels + */ +uint8_t AP_Radio_cypress::num_channels(void) +{ + uint32_t now = AP_HAL::millis(); + uint8_t ch = get_rssi_chan(); + if (ch > 0) { + dsm.pwm_channels[ch-1] = dsm.rssi; + dsm.num_channels = MAX(dsm.num_channels, ch); + } + + ch = get_pps_chan(); + if (ch > 0) { + dsm.pwm_channels[ch-1] = t_status.pps; + dsm.num_channels = MAX(dsm.num_channels, ch); + } + + ch = get_tx_rssi_chan(); + if (ch > 0) { + dsm.pwm_channels[ch-1] = dsm.tx_rssi; + dsm.num_channels = MAX(dsm.num_channels, ch); + } + + ch = get_tx_pps_chan(); + if (ch > 0) { + dsm.pwm_channels[ch-1] = dsm.tx_pps; + dsm.num_channels = MAX(dsm.num_channels, ch); + } + + if (now - last_debug_print_ms > 1000) { + last_debug_print_ms = now; + if (get_debug_level() > 1) { + print_debug_info(); + } + + t_status.pps = stats.recv_packets - last_stats.recv_packets; + t_status.rssi = (uint8_t)dsm.rssi; + last_stats = stats; + } + + return dsm.num_channels; +} + +/* + send a fwupload ack if needed + */ +void AP_Radio_cypress::check_fw_ack(void) +{ + Debug(4,"check need_ack\n"); + if (fwupload.need_ack && sem->take_nonblocking()) { + // ack the send of a DATA96 fw packet to TX + fwupload.need_ack = false; + uint8_t data16[16] {}; + uint32_t ack_to = fwupload.offset + fwupload.acked; + memcpy(&data16[0], &ack_to, 4); + mavlink_msg_data16_send(fwupload.chan, 42, 4, data16); + Debug(4,"sent ack DATA16\n"); + sem->give(); + } +} + +/* + return time of last receive in microseconds + */ +uint32_t AP_Radio_cypress::last_recv_us(void) +{ + // we use the parse time, so it matches when channel values are filled in + return dsm.last_parse_us; +} + +/* + send len bytes as a single packet + */ +bool AP_Radio_cypress::send(const uint8_t *pkt, uint16_t len) +{ + // disabled for now + return false; +} + +/* 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} +}, +}; +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 +}; + +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 +}; +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 +}; + +/* + read radio status, handling the race condition between completion and error + */ +uint8_t AP_Radio_cypress::read_status_debounced(uint8_t adr) +{ + uint8_t ret; + + dev->set_chip_select(true); + ret = read_register(adr); + + // If COMPLETE and ERROR bits mismatch, then re-read register + if ((ret & (CYRF_RXC_IRQ | CYRF_RXE_IRQ)) != 0 + && (ret & (CYRF_RXC_IRQ | CYRF_RXE_IRQ)) != (CYRF_RXC_IRQ | CYRF_RXE_IRQ)) { + uint8_t v2; + dev->read(&v2, 1); + ret |= v2; // re-read and make bits sticky + } + dev->set_chip_select(false); + return ret; +} + +/* + force the initial state of the radio + */ +bool AP_Radio_cypress::force_initial_state(void) +{ + const uint32_t force_initial_state_start_ms = AP_HAL::millis(); + while (AP_HAL::millis() - force_initial_state_start_ms < 100) { + write_register(CYRF_XACT_CFG, CYRF_FRC_END); + uint32_t start_ms = AP_HAL::millis(); + do { + if ((read_register(CYRF_XACT_CFG) & CYRF_FRC_END) == 0) { + return true; // FORCE_END done (osc running) + } + } while (AP_HAL::millis() - start_ms < 5); + + // FORCE_END failed to complete, implying going SLEEP to IDLE and + // oscillator failed to start. Recover by returning to SLEEP and + // trying to start oscillator again. + write_register(CYRF_XACT_CFG, CYRF_MODE_SLEEP); + } + return false; +} + +/* + set desired channel + */ +void AP_Radio_cypress::set_channel(uint8_t channel) +{ + if (dsm.forced_channel != -1) { + channel = dsm.forced_channel; + } + write_register(CYRF_CHANNEL, channel); +} + +void AP_Radio_cypress::radio_set_config(const struct config *conf, uint8_t size) +{ + // setup required radio config + for (uint8_t i=0; idelay(10); + } + if (i == 1000) { + Debug(1, "Cypress: radio_init failed\n"); + return; + } + + // base config + radio_set_config(cyrf_config, ARRAY_SIZE(cyrf_config)); + + // start with receive config + radio_set_config(cyrf_transfer_config, ARRAY_SIZE(cyrf_transfer_config)); + + if (get_disable_crc()) { + write_register(CYRF_RX_OVERRIDE, CYRF_DIS_RXCRC); + } + + dsm_setup_transfer_dsmx(); + + write_register(CYRF_XTAL_CTRL,0x80); // XOUT=BitSerial + if (!force_initial_state()) { + Debug(1, "Cypress: radio_init failed\n"); + return; + } + write_register(CYRF_PWR_CTRL,0x20); // Disable PMU + + // start in RECV state + state = STATE_RECV; + + Debug(1, "Cypress: radio_init done\n"); + + start_receive(); + + // setup handler for rising edge of IRQ pin +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + stm32_gpiosetevent(CYRF_IRQ_INPUT, true, false, false, irq_radio_trampoline); +#endif +} + +void AP_Radio_cypress::dump_registers(uint8_t n) +{ + for (uint8_t i=0; iread_registers(reg, &v, 1); + return v; +} + + +/* + write multiple bytes + */ +void AP_Radio_cypress::write_multiple(uint8_t reg, uint8_t n, const uint8_t *data) +{ + dev->set_chip_select(true); + reg |= FLAG_WRITE; + dev->transfer(®, 1, nullptr, 0); + dev->transfer(data, n, nullptr, 0); + dev->set_chip_select(false); +} + +/* + write one register value + */ +void AP_Radio_cypress::write_register(uint8_t reg, uint8_t value) +{ + dev->write_register(reg | FLAG_WRITE, value); +} + + +/* + support all 4 rc input modes by swapping channels. + */ +void AP_Radio_cypress::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_cypress::check_double_bind(void) +{ + if (dsm.tx_pps <= dsm.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"); + memset(dsm.mfg_id, 1, sizeof(dsm.mfg_id)); + dsm.last_recv_us = 0; + dsm_setup_transfer_dsmx(); +} + +/* + parse channels from a packet + */ +bool AP_Radio_cypress::parse_dsm_channels(const uint8_t *data) +{ + uint16_t num_values = 0; + uint16_t pwm_channels[max_channels] {}; + + // 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, + &num_values, + ARRAY_SIZE(pwm_channels))) { + // invalid packet + Debug(2, "DSM: bad decode\n"); + return false; + } + if (num_values < 5) { + Debug(2, "DSM: num_values=%u\n", num_values); + return false; + } + + // cope with mode1/mode2 + map_stick_mode(pwm_channels); + + 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; + + if (num_values == 8) { + // decode telemetry ack value and version + uint16_t d=0; + if (is_DSM2()) { + d = data[14] << 8 | data[15]; + } else { + // see chan_order[] for DSMX + d = data[10] << 8 | data[11]; + } + // extra data is sent on channel 8, with 3 bit key and 8 bit data + uint8_t chan = d>>11; + uint8_t key = (d >> 8) & 0x7; + uint8_t v = d & 0xFF; + if (chan == 7 && key == 0) { + // got an ack from key 0 + Debug(4, "ack %u seq=%u acked=%u length=%u len=%u\n", + v, fwupload.sequence, fwupload.acked, fwupload.length, fwupload.len); + if (fwupload.sequence == v && 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(); + } + } + if (chan == 7) { + // extract telemetry extra data + switch (key) { + case 1: + dsm.tx_firmware_year = v; + break; + case 2: + dsm.tx_firmware_month = v; + break; + case 3: + dsm.tx_firmware_day = v; + break; + case 4: + dsm.tx_rssi = v; + break; + case 5: + dsm.tx_pps = v; + dsm.have_tx_pps = true; + check_double_bind(); + break; + case 6: + if (v != dsm.tx_bl_version) { + if (v == 2) { + // TX with new filter gets a default power of 6 + set_tx_max_power_default(6); + } + } + dsm.tx_bl_version = v; + break; + } + } + } + return true; +} + +/* + process an incoming bind packet + */ +void AP_Radio_cypress::process_bind(const uint8_t *pkt, uint8_t len) +{ + if (len != 16) { + return; + } + bool ok = (len==16 && pkt[0] == pkt[4] && pkt[1] == pkt[5] && pkt[2] == pkt[6] && pkt[3] == pkt[7]); + + // Calculate the first sum + uint16_t bind_sum = 384 - 0x10; + for (uint8_t i = 0; i < 8; i++) { + bind_sum += pkt[i]; + } + + // Check the first sum + if (pkt[8] != bind_sum >> 8 || pkt[9] != (bind_sum & 0xFF)) { + ok = false; + } + + // Calculate second sum + for (uint8_t i = 8; i < 14; i++) { + bind_sum += pkt[i]; + } + + // Check the second sum + if (pkt[14] != bind_sum >> 8 || pkt[15] != (bind_sum & 0xFF)) { + ok = false; + } + + if (state == STATE_AUTOBIND) { + uint8_t rssi = read_register(CYRF_RSSI) & 0x1F; + Debug(3,"bind RSSI %u\n", rssi); + if (rssi < get_autobind_rssi()) { + 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]; + uint8_t protocol = pkt[12]; + (void)num_chan; + // change to normal receive + memcpy(dsm.mfg_id, mfg_id, 4); + state = STATE_RECV; + + radio_set_config(cyrf_transfer_config, ARRAY_SIZE(cyrf_transfer_config)); + + if (get_disable_crc()) { + write_register(CYRF_RX_OVERRIDE, CYRF_DIS_RXCRC); + } + + dsm.protocol = (enum dsm_protocol)protocol; + dsm_setup_transfer_dsmx(); + + Debug(1, "BIND OK: mfg_id={0x%02x, 0x%02x, 0x%02x, 0x%02x} N=%u P=0x%02x DSM2=%u\n", + mfg_id[0], mfg_id[1], mfg_id[2], mfg_id[3], + num_chan, + protocol, + is_DSM2()); + + dsm.last_recv_us = AP_HAL::micros(); + + if (is_DSM2()) { + dsm2_start_sync(); + } + + dsm.need_bind_save = true; + } +} + +/* + start DSM2 sync + */ +void AP_Radio_cypress::dsm2_start_sync(void) +{ + uint8_t factory_test = get_factory_test(); + if (factory_test != 0) { + dsm.channels[0] = (factory_test*7) % DSM_MAX_CHANNEL; + dsm.channels[1] = (dsm.channels[0] + 5) % DSM_MAX_CHANNEL; + dsm.sync = DSM2_OK; + } else { + Debug(2, "DSM2 start sync\n"); + dsm.sync = DSM2_SYNC_A; + } +} + +/* + process an incoming packet + */ +void AP_Radio_cypress::process_packet(const uint8_t *pkt, uint8_t len) +{ + if (len == 16) { + 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 { + ok = (pkt[0] == id[2] && pkt[1] == id[3]); + } + if (ok && is_DSM2() && dsm.sync < DSM2_OK) { + if (dsm.sync == DSM2_SYNC_A) { + dsm.channels[0] = dsm.current_rf_channel; + dsm.sync = DSM2_SYNC_B; + Debug(2, "DSM2 SYNCA chan=%u\n", dsm.channels[0]); + dsm.last_recv_us = now; + } else { + if (dsm.current_rf_channel != dsm.channels[0]) { + dsm.channels[1] = dsm.current_rf_channel; + dsm.sync = DSM2_OK; + Debug(2, "DSM2 SYNCB chan=%u\n", dsm.channels[1]); + dsm.last_recv_us = now; + } + } + return; + } + if (ok && (!is_DSM2() || dsm.sync >= DSM2_SYNC_B)) { + ok = parse_dsm_channels(pkt); + } + if (ok) { + uint32_t packet_dt_us = now - dsm.last_recv_us; + + dsm.last_recv_chan = dsm.current_channel; + dsm.last_recv_us = now; + if (dsm.crc_errors > 2) { + dsm.crc_errors -= 2; + } + + stats.recv_packets++; + + // sample the RSSI + uint8_t rssi = read_register(CYRF_RSSI) & 0x1F; + dsm.rssi = 0.95 * dsm.rssi + 0.05 * rssi; + + if (packet_dt_us < 5000) { + dsm.pkt_time1 = packet_dt_us; + } 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)) { + /* + we have just received two packets rapidly, which + means we have about 7ms before the next + one. That gives time for a telemetry packet. We + send it 1ms after we receive the incoming packet + + If auto-bind is enabled we don't send telemetry + till we've received a tx_pps value from the + TX. This allows us to detect double binding (two + RX bound to the same TX) + */ + state = STATE_SEND_TELEM; +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + hrt_call_after(&wait_call, 1000, (hrt_callout)irq_timeout_trampoline, nullptr); +#endif + } + } + } else { + stats.bad_packets++; + } + } else { + stats.bad_packets++; + } +} + + +/* + start packet receive + */ +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); + + dsm.receive_start_us = AP_HAL::micros(); + if (state == STATE_AUTOBIND) { + dsm.receive_timeout_usec = 90000; + } else if (state == STATE_BIND) { + dsm.receive_timeout_usec = 15000; + } else { + dsm.receive_timeout_usec = 12000; + } +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + //dsm.receive_timeout_usec = 45000; + hrt_call_after(&wait_call, dsm.receive_timeout_usec, (hrt_callout)irq_timeout_trampoline, nullptr); +#endif +} + +/* + handle a receive IRQ + */ +void AP_Radio_cypress::irq_handler_recv(uint8_t rx_status) +{ + if ((rx_status & (CYRF_RXC_IRQ | CYRF_RXE_IRQ)) == 0) { + // nothing interesting yet + return; + } + + uint8_t pkt[16]; + uint8_t rlen = read_register(CYRF_RX_COUNT); + if (rlen > 16) { + rlen = 16; + } + if (rlen > 0) { + dev->read_registers(CYRF_RX_BUFFER, pkt, rlen); + } + + if (rx_status & CYRF_RXE_IRQ) { + uint8_t reason = read_register(CYRF_RX_STATUS); + if (reason & CYRF_BAD_CRC) { + dsm.crc_errors++; + if (dsm.crc_errors > 20) { + Debug(2, "Flip CRC\n"); + // flip crc seed, this allows us to resync with transmitter + dsm.crc_seed = ~dsm.crc_seed; + dsm.crc_errors = 0; + } + } + write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END); + write_register(CYRF_RX_ABORT, 0); + stats.recv_errors++; + } else if (rx_status & CYRF_RXC_IRQ) { + if (state == STATE_RECV) { + process_packet(pkt, rlen); + } else { + process_bind(pkt, rlen); + } + } + + if (state == STATE_AUTOBIND) { + state = STATE_RECV; + } + + if (state != STATE_SEND_TELEM) { + start_receive(); + } +} + + +/* + handle a send IRQ + */ +void AP_Radio_cypress::irq_handler_send(uint8_t tx_status) +{ + if ((tx_status & (CYRF_TXC_IRQ | CYRF_TXE_IRQ)) == 0) { + // nothing interesting yet + return; + } + state = STATE_RECV; + start_receive(); +} + + +/* + IRQ handler + */ +void AP_Radio_cypress::irq_handler(void) +{ + //hal.console->printf("IRQ\n"); + if (!dev->get_semaphore()->take_nonblocking()) { + // we have to wait for timeout instead + return; + } + // always read both rx and tx status. This ensure IRQ is cleared + uint8_t rx_status = read_status_debounced(CYRF_RX_IRQ_STATUS); + uint8_t tx_status = read_status_debounced(CYRF_TX_IRQ_STATUS); + + switch (state) { + case STATE_AUTOBIND: + // fallthrough + case STATE_RECV: + case STATE_BIND: + irq_handler_recv(rx_status); + break; + + case STATE_SEND_TELEM: + case STATE_SEND_TELEM_WAIT: + irq_handler_send(tx_status); + break; + + case STATE_SEND_FCC: + // stop transmit oscillator + 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; + } + dev->get_semaphore()->give(); +} + +/* + called on radio timeout + */ +void AP_Radio_cypress::irq_timeout(void) +{ + stats.timeouts++; + if (!dev->get_semaphore()->take_nonblocking()) { + // schedule a new timeout +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + hrt_call_after(&wait_call, dsm.receive_timeout_usec, (hrt_callout)irq_timeout_trampoline, nullptr); +#endif + return; + } + + if (get_fcc_test() != 0 && state != STATE_SEND_FCC) { + Debug(3,"Starting FCC test\n"); + state = STATE_SEND_FCC; + } else if (get_fcc_test() == 0 && state == STATE_SEND_FCC) { + Debug(3,"Ending FCC test\n"); + state = STATE_RECV; + } + + switch (state) { + case STATE_SEND_TELEM: + send_telem_packet(); + break; + case STATE_SEND_FCC: + send_FCC_test_packet(); + break; + case STATE_AUTOBIND: + case STATE_SEND_TELEM_WAIT: + state = STATE_RECV; + // fall through + default: + write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END); + write_register(CYRF_RX_ABORT, 0); + start_receive(); + break; + } + + dev->get_semaphore()->give(); +} + + +/* + called on rising edge of radio IRQ pin + */ +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +int AP_Radio_cypress::irq_radio_trampoline(int irq, void *context) +{ + radio_instance->irq_handler(); + return 0; +} +#endif + +/* + called on HRT timeout + */ +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +int AP_Radio_cypress::irq_timeout_trampoline(int irq, void *context) +{ + radio_instance->irq_timeout(); + return 0; +} +#endif +/* + Set the current DSM channel with SOP, CRC and data code + */ +void AP_Radio_cypress::dsm_set_channel(uint8_t channel, bool is_dsm2, uint8_t sop_col, uint8_t data_col, uint16_t crc_seed) +{ + //printf("dsm_set_channel: %u\n", channel); + + uint8_t pn_row; + pn_row = is_dsm2? channel % 5 : (channel-2) % 5; + + // set CRC seed + write_register(CYRF_CRC_SEED_LSB, crc_seed & 0xff); + write_register(CYRF_CRC_SEED_MSB, crc_seed >> 8); + + // set start of packet code + if (memcmp(dsm.last_sop_code, pn_codes[pn_row][sop_col], 8) != 0) { + write_multiple(CYRF_SOP_CODE, 8, pn_codes[pn_row][sop_col]); + memcpy(dsm.last_sop_code, pn_codes[pn_row][sop_col], 8); + } + + // set data code + if (memcmp(dsm.last_data_code, pn_codes[pn_row][data_col], 16) != 0) { + write_multiple(CYRF_DATA_CODE, 16, pn_codes[pn_row][data_col]); + memcpy(dsm.last_data_code, pn_codes[pn_row][data_col], 16); + } + + if (get_disable_crc() != dsm.last_discrc) { + dsm.last_discrc = get_disable_crc(); + Debug(3,"Cypress: DISCRC=%u\n", dsm.last_discrc); + write_register(CYRF_RX_OVERRIDE, dsm.last_discrc?CYRF_DIS_RXCRC:0); + } + + if (get_transmit_power() != dsm.last_transmit_power+1) { + dsm.last_transmit_power = get_transmit_power()-1; + 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); +} + +/* + Generate the DSMX channels from the manufacturer ID + */ +void AP_Radio_cypress::dsm_generate_channels_dsmx(uint8_t mfg_id[4], uint8_t channels[23]) +{ + // Calculate the DSMX channels + int idx = 0; + uint32_t id = ~((mfg_id[0] << 24) | (mfg_id[1] << 16) | + (mfg_id[2] << 8) | (mfg_id[3] << 0)); + uint32_t id_tmp = id; + + // While not all channels are set + while(idx < 23) { + int i; + int count_3_27 = 0, count_28_51 = 0, count_52_76 = 0; + + id_tmp = id_tmp * 0x0019660D + 0x3C6EF35F; // Randomization + uint8_t next_ch = ((id_tmp >> 8) % 0x49) + 3; // Use least-significant byte and must be larger than 3 + if (((next_ch ^ id) & 0x01 ) == 0) { + continue; + } + + // Go trough all already set channels + for (i = 0; i < idx; i++) { + // Channel is already used + if (channels[i] == next_ch) { + break; + } + + // Count the channel groups + if(channels[i] <= 27) { + count_3_27++; + } else if (channels[i] <= 51) { + count_28_51++; + } else { + count_52_76++; + } + } + + // When channel is already used continue + if (i != idx) { + continue; + } + + // Set the channel when channel groups aren't full + if ((next_ch < 28 && count_3_27 < 8) // Channels 3-27: max 8 + || (next_ch >= 28 && next_ch < 52 && count_28_51 < 7) // Channels 28-52: max 7 + || (next_ch >= 52 && count_52_76 < 8)) { // Channels 52-76: max 8 + channels[idx++] = next_ch; + } + } + + Debug(2, "Generated DSMX channels\n"); +} + +/* + setup for DSMX transfers + */ +void AP_Radio_cypress::dsm_setup_transfer_dsmx(void) +{ + dsm.current_channel = 0; + + dsm.crc_seed = ~((dsm.mfg_id[0] << 8) + dsm.mfg_id[1]); + dsm.sop_col = (dsm.mfg_id[0] + dsm.mfg_id[1] + dsm.mfg_id[2] + 2) & 0x07; + dsm.data_col = 7 - dsm.sop_col; + + dsm_generate_channels_dsmx(dsm.mfg_id, dsm.channels); +} + +/* + choose channel to receive on + */ +void AP_Radio_cypress::dsm_choose_channel(void) +{ + uint32_t now = AP_HAL::micros(); + uint32_t dt = now - dsm.last_recv_us; + 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 + dsm.current_rf_channel |= 1; + dsm.current_rf_channel = (dsm.current_rf_channel+2) % DSM_MAX_CHANNEL; + dsm.last_chan_change_us = now; + } + set_channel(dsm.current_rf_channel); + return; + } + + if (get_autobind_time() != 0 && + dsm.last_recv_us == 0 && + now - dsm.last_autobind_send > 300*1000UL && + now > get_autobind_time() * 1000*1000UL && + get_factory_test() == 0 && + state == STATE_RECV) { + // try to receive an auto-bind packet + dsm_set_channel(AUTOBIND_CHANNEL, true, 0, 0, 0); + + state = STATE_AUTOBIND; + + Debug(3,"recv autobind %u\n", 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 + dsm.current_rf_channel &= ~1; + dsm.current_rf_channel = (dsm.current_rf_channel+2) % DSM_MAX_CHANNEL; + dsm.last_chan_change_us = now; + } + //hal.console->printf("%u chan=%u\n", AP_HAL::micros(), dsm.current_rf_channel); + dsm_set_channel(dsm.current_rf_channel, is_DSM2(), + dsm.sop_col, dsm.data_col, + 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; + } else if (dt > 20*cycle_time) { + // change channel slowly + next_channel = dsm.last_recv_chan + (dt / (cycle_time*2)); + } else { + // predict next channel + next_channel = dsm.last_recv_chan + 1; + next_channel += (dt / cycle_time) * 2; + if (dt % cycle_time > (unsigned)(dsm.pkt_time1 + 1000U)) { + next_channel++; + } + } + + uint8_t chan_count = is_DSM2()?2:23; + dsm.current_channel = next_channel; + if (dsm.current_channel >= chan_count) { + dsm.current_channel %= chan_count; + if (!is_DSM2()) { + dsm.crc_seed = ~dsm.crc_seed; + } + } + + if (is_DSM2() && dsm.sync == DSM2_SYNC_B && dsm.current_channel == 1) { + // scan to next channelb + do { + dsm.channels[1] &= ~1; + 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; + if (dsm.current_channel & 1) { + seed = ~seed; + } + + if (is_DSM2()) { + if (now - dsm.last_recv_us > 5000000) { + dsm2_start_sync(); + } + } + + dsm_set_channel(dsm.current_rf_channel, is_DSM2(), + dsm.sop_col, dsm.data_col, seed); +} + +/* + setup radio for bind + */ +void AP_Radio_cypress::start_recv_bind(void) +{ + if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + // shouldn't be possible + return; + } + + Debug(1, "Cypress: start_recv_bind\n"); + + 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)); + + write_register(CYRF_CRC_SEED_LSB, 0); + write_register(CYRF_CRC_SEED_MSB, 0); + + write_multiple(CYRF_SOP_CODE, 8, pn_codes[0][0]); + + uint8_t data_code[16]; + memcpy(data_code, pn_codes[0][8], 8); + memcpy(&data_code[8], pn_bind, 8); + write_multiple(CYRF_DATA_CODE, 16, data_code); + + dsm.current_rf_channel = 1; + + start_receive(); + + dev->get_semaphore()->give(); +} + +/* + save bind info + */ +void AP_Radio_cypress::save_bind_info(void) +{ + // access to storage for bind information + StorageAccess bind_storage(StorageManager::StorageBindInfo); + struct bind_info info; + + info.magic = bind_magic; + memcpy(info.mfg_id, dsm.mfg_id, sizeof(info.mfg_id)); + info.protocol = dsm.protocol; + + if (bind_storage.write_block(0, &info, sizeof(info))) { + dsm.need_bind_save = false; + } +} + +/* + load bind info + */ +void AP_Radio_cypress::load_bind_info(void) +{ + // access to storage for bind information + StorageAccess bind_storage(StorageManager::StorageBindInfo); + 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)); + dsm.mfg_id[0] = factory_test; + dsm.protocol = DSM_DSM2_2; + dsm2_start_sync(); + } else if (bind_storage.read_block(&info, 0, sizeof(info)) && info.magic == bind_magic) { + memcpy(dsm.mfg_id, info.mfg_id, sizeof(info.mfg_id)); + dsm.protocol = info.protocol; + } +} + +bool AP_Radio_cypress::is_DSM2(void) +{ + if (get_protocol() != AP_Radio::PROTOCOL_AUTO) { + return get_protocol() == AP_Radio::PROTOCOL_DSM2; + } + return dsm.protocol == DSM_DSM2_1 || dsm.protocol == DSM_DSM2_2; +} + +/* + transmit a 16 byte packet + this is a blind send, not waiting for ack or completion +*/ +void AP_Radio_cypress::transmit16(const uint8_t data[16]) +{ + write_register(CYRF_TX_LENGTH, 16); + write_register(CYRF_TX_CTRL, CYRF_TX_CLR); + + write_multiple(CYRF_TX_BUFFER, 16, data); + write_register(CYRF_TX_CTRL, CYRF_TX_GO | CYRF_TXC_IRQEN); + dsm.send_count++; +} + + +/* + send a telemetry structure packet + */ +void AP_Radio_cypress::send_telem_packet(void) +{ + struct telem_packet pkt; + + 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(); + + // send fw update packet for 7/8 of packets if any data pending + if (fwupload.length != 0 && + fwupload.length > fwupload.acked && + ((fwupload.counter++ & 0x07) != 0) && + sem->take_nonblocking()) { + 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; + 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); + sem->give(); + pkt.crc = crc_crc8((const uint8_t *)&pkt.type, 15); + } else { + pkt.type = TELEM_STATUS; + pkt.payload.status = t_status; + 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); + + state = STATE_SEND_TELEM_WAIT; +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + hrt_call_after(&wait_call, 2000, (hrt_callout)irq_timeout_trampoline, nullptr); +#endif +} + +/* + send a FCC test packet + */ +void AP_Radio_cypress::send_FCC_test_packet(void) +{ + uint8_t pkt[16] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }; + + state = STATE_SEND_FCC; + + uint8_t channel=0; + + switch (get_fcc_test()) { + case 0: + // switch back to normal operation + dsm.forced_channel = -1; + send_telem_packet(); + return; + case 1: + case 4: + channel = DSM_SCAN_MIN_CH; + break; + case 2: + case 5: + channel = DSM_SCAN_MID_CH; + break; + case 3: + case 6: + default: + channel = DSM_SCAN_MAX_CH; + break; + } + + Debug(5,"FCC send %u\n", channel); + + 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)); + + set_channel(channel); + } + +#if FCC_SUPPORT_CW_MODE + if (get_fcc_test() > 3) { + // continuous preamble transmit is closest approximation to CW + // that is possible with this chip + 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); + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + hrt_call_after(&wait_call, 500000, (hrt_callout)irq_timeout_trampoline, nullptr); +#endif + } else { + write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_TX | CYRF_FRC_END); + write_register(CYRF_RX_ABORT, 0); + transmit16(pkt); +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + hrt_call_after(&wait_call, 10000, (hrt_callout)irq_timeout_trampoline, nullptr); +#endif + } +#else + write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_TX | CYRF_FRC_END); + write_register(CYRF_RX_ABORT, 0); + transmit16(pkt); +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + hrt_call_after(&wait_call, 10000, (hrt_callout)irq_timeout_trampoline, nullptr); +#endif +#endif +} + +#endif // HAL_RCINPUT_WITH_AP_RADIO + diff --git a/libraries/AP_Radio/AP_Radio_cypress.h b/libraries/AP_Radio/AP_Radio_cypress.h new file mode 100644 index 0000000000..7fca61e19a --- /dev/null +++ b/libraries/AP_Radio/AP_Radio_cypress.h @@ -0,0 +1,278 @@ +/* + 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 Cypress 2.4GHz radio. + + With thanks to the SuperBitRF project + See http://wiki.paparazziuav.org/wiki/SuperbitRF + + This implementation uses the DSMX protocol on a CYRF6936 + */ + +#include "AP_Radio_backend.h" +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#include +#include +#include +#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; + + // start bind process as a receiver + void start_recv_bind(void) override; + + // return time in microseconds of last received R/C packet + uint32_t last_recv_us(void) override; + + // return number of input channels + uint8_t num_channels(void) override; + + // return current PWM of a channel + uint16_t read(uint8_t chan) override; + + // update status + void update(void) override; + + // get TX fw version + 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) { + t_status.wifi_chan = channel; + } + +private: + AP_HAL::OwnPtr dev; + static AP_Radio_cypress *radio_instance; + + void radio_init(void); + + void dump_registers(uint8_t n); + + bool 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); + + enum { + STATE_RECV, + STATE_BIND, + STATE_AUTOBIND, + STATE_SEND_TELEM, + STATE_SEND_TELEM_WAIT, + STATE_SEND_FCC + } state; + + struct config { + uint8_t reg; + uint8_t value; + }; + static const uint8_t pn_codes[5][9][8]; + static const uint8_t pn_bind[]; + static const config cyrf_config[]; + static const config cyrf_bind_config[]; + static const config cyrf_transfer_config[]; + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + sem_t irq_sem; + struct hrt_call wait_call; +#endif + void radio_set_config(const struct config *config, uint8_t size); + + void start_receive(void); + + // main IRQ handler + void irq_handler(void); + + // IRQ handler for packet receive + void irq_handler_recv(uint8_t rx_status); + + // 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_PX4 + static int irq_radio_trampoline(int irq, void *context); + static int irq_timeout_trampoline(int irq, void *context); +#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; + + enum dsm_protocol { + DSM_NONE = 0, // not bound yet + DSM_DSM2_1 = 0x01, // The original DSM2 protocol with 1 packet of data + DSM_DSM2_2 = 0x02, // The original DSM2 protocol with 2 packets of data + DSM_DSMX_1 = 0xA2, // The original DSMX protocol with 1 packet of data + DSM_DSMX_2 = 0xB2, // The original DSMX protocol with 2 packets of data + }; + + enum dsm2_sync { + DSM2_SYNC_A, + DSM2_SYNC_B, + DSM2_OK + }; + + // semaphore between ISR and main thread + AP_HAL::Semaphore *sem; + + // dsm config data and status + struct { + uint8_t channels[23]; + enum dsm_protocol protocol; + uint8_t mfg_id[4]; + uint8_t current_channel; + uint8_t current_rf_channel; + uint16_t crc_seed; + uint8_t sop_col; + uint8_t data_col; + uint8_t last_sop_code[8]; + uint8_t last_data_code[16]; + + uint32_t receive_start_us; + uint32_t receive_timeout_usec; + + uint32_t last_recv_us; + uint32_t last_parse_us; + uint32_t last_recv_chan; + uint32_t last_chan_change_us; + uint16_t num_channels; + uint16_t pwm_channels[max_channels]; + bool need_bind_save; + enum dsm2_sync sync; + uint32_t crc_errors; + float rssi; + bool last_discrc; + uint8_t last_transmit_power; + uint32_t send_irq_count; + uint32_t send_count; + uint16_t pkt_time1 = 3000; + uint16_t pkt_time2 = 7000; + uint8_t tx_firmware_year; + uint8_t tx_firmware_month; + uint8_t tx_firmware_day; + int8_t forced_channel = -1; + uint8_t tx_rssi; + uint8_t tx_pps; + uint32_t last_autobind_send; + bool have_tx_pps; + uint32_t telem_send_count; + uint8_t tx_bl_version; + } dsm; + + struct { + mavlink_channel_t chan; + bool need_ack; + uint8_t counter; + uint8_t sequence; + uint32_t offset; + uint32_t length; + uint32_t acked; + uint8_t len; + enum telem_type fw_type; + uint8_t pending_data[92]; + } fwupload; + + // bind structure saved to storage + static const uint16_t bind_magic = 0x43F6; + struct PACKED bind_info { + uint16_t magic; + uint8_t mfg_id[4]; + enum dsm_protocol protocol; + }; + + 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); + + // generate DSMX channels + void dsm_generate_channels_dsmx(uint8_t mfg_id[4], uint8_t channels[23]); + + // setup for DSMX transfers + void dsm_setup_transfer_dsmx(void); + + // choose channel to receive on + void dsm_choose_channel(void); + + // 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); + + // process an incoming packet + void process_packet(const uint8_t *pkt, uint8_t len); + + // process an incoming bind packet + void process_bind(const uint8_t *pkt, uint8_t len); + + // load bind info from storage + void load_bind_info(void); + + // save bind info to storage + void save_bind_info(void); + + bool is_DSM2(void); + + // send a 16 byte packet + void transmit16(const uint8_t data[16]); + + void send_telem_packet(void); + 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); + + // re-sync DSM2 + void dsm2_start_sync(void); + + // check for double binding + void check_double_bind(void); +}; + diff --git a/libraries/AP_Radio/examples/DSMReceiver/DSMReceiver.cpp b/libraries/AP_Radio/examples/DSMReceiver/DSMReceiver.cpp new file mode 100644 index 0000000000..e67217266d --- /dev/null +++ b/libraries/AP_Radio/examples/DSMReceiver/DSMReceiver.cpp @@ -0,0 +1,49 @@ +/* + * DSM receiver example code for Cypress radio + * + * With thanks to the SuperBitRF project and their excellent cyrf6936 dongle: + * https://1bitsquared.com/products/superbit-usbrf-dongle + */ + +#include +#include +#include + +#define debug(fmt, args...) printf(fmt, ##args) + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +static AP_Radio radio; + +static bool do_bind = false; + +void setup() +{ + hal.console->begin(115200); + debug("RADIO init\n"); + hal.scheduler->delay(1000); + radio.init(); + if (do_bind) { + radio.start_recv_bind(); + } +} + +void loop() +{ + static AP_Radio::stats stats; + hal.scheduler->delay(1000); + + AP_Radio::stats new_stats = radio.get_stats(); + debug("recv:%3u bad:%3u to:%3u re:%u N:%2u 1:%4u 1:%4u 3:%4u 4:%4u 5:%4u 6:%4u 7:%4u 8:%4u 14:%u\n", + new_stats.recv_packets - stats.recv_packets, + new_stats.bad_packets - stats.bad_packets, + new_stats.timeouts - stats.timeouts, + new_stats.recv_errors - stats.recv_errors, + radio.num_channels(), + radio.read(0), radio.read(1), radio.read(2), radio.read(3), + radio.read(4), radio.read(5), radio.read(6), radio.read(7), + radio.read(13)); + stats = new_stats; +} + +AP_HAL_MAIN(); diff --git a/libraries/AP_Radio/examples/DSMReceiver/wscript b/libraries/AP_Radio/examples/DSMReceiver/wscript new file mode 100644 index 0000000000..719ec151ba --- /dev/null +++ b/libraries/AP_Radio/examples/DSMReceiver/wscript @@ -0,0 +1,7 @@ +#!/usr/bin/env python +# encoding: utf-8 + +def build(bld): + bld.ap_example( + use='ap', + ) diff --git a/libraries/AP_Radio/telem_structure.h b/libraries/AP_Radio/telem_structure.h new file mode 100644 index 0000000000..7494aacd42 --- /dev/null +++ b/libraries/AP_Radio/telem_structure.h @@ -0,0 +1,74 @@ +/* + structures for telemetry packets + This header is common to ArduPilot AP_Radio and STM8 TX code + */ + +#pragma once + +enum telem_type { + TELEM_STATUS= 0, // a telem_status packet + TELEM_PLAY = 1, // play a tune + TELEM_FW = 2, // update new firmware +}; + +#define TELEM_FLAG_GPS_OK (1U<<0) +#define TELEM_FLAG_ARM_OK (1U<<1) +#define TELEM_FLAG_BATT_OK (1U<<2) +#define TELEM_FLAG_ARMED (1U<<4) +#define TELEM_FLAG_POS_OK (1U<<5) +#define TELEM_FLAG_VIDEO (1U<<6) +#define TELEM_FLAG_HYBRID (1U<<7) + +struct 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 wifi_chan; + uint8_t tx_max; + uint8_t note_adjust; +}; + +// play a tune +struct telem_play { + uint8_t seq; + uint8_t tune_index; +}; + +// write to new firmware +struct telem_firmware { + uint8_t seq; + uint8_t len; + uint16_t offset; + uint8_t data[8]; +}; + +/* + telemetry packet from RX to TX + */ +struct telem_packet { + uint8_t crc; // simple CRC + enum telem_type type; + union { + uint8_t pkt[14]; + struct telem_status status; + struct telem_firmware fw; + } payload; +}; + + +enum tx_telem_type { + TXTELEM_RSSI = 0, + TXTELEM_CRC1 = 1, + TXTELEM_CRC2 = 2, +}; + +/* + tx_status structure sent one byte at a time to RX. This is packed + into channels 8, 9 and 10 (using 32 bits of a possible 33) + */ +struct telem_tx_status { + uint8_t crc; + enum tx_telem_type type; + uint16_t data; +};