Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Radio/AP_Radio_cc2500.cpp

1087 lines
33 KiB
C++
Raw Blame History

/*
driver for TI CC2500 radio
Many thanks to the cleanflight and betaflight projects
*/
#include <AP_HAL/AP_HAL.h>
// #pragma GCC optimize("O0")
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#if HAL_RCINPUT_WITH_AP_RADIO
#include <AP_Math/AP_Math.h>
#include "AP_Radio_cc2500.h"
#include <utility>
#include <stdio.h>
#include <StorageManager/StorageManager.h>
#include <AP_Notify/AP_Notify.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Math/crc.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
static THD_WORKING_AREA(_irq_handler_wa, 512);
#define TIMEOUT_PRIORITY 250 //Right above timer thread
#define EVT_TIMEOUT EVENT_MASK(0)
#define EVT_IRQ EVENT_MASK(1)
#define EVT_BIND EVENT_MASK(2)
#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
// object instance for trampoline
AP_Radio_cc2500 *AP_Radio_cc2500::radio_instance;
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
thread_t *AP_Radio_cc2500::_irq_handler_ctx;
virtual_timer_t AP_Radio_cc2500::timeout_vt;
uint32_t AP_Radio_cc2500::irq_time_us;
#endif
/*
constructor
*/
AP_Radio_cc2500::AP_Radio_cc2500(AP_Radio &_radio) :
AP_Radio_backend(_radio),
cc2500(hal.spi->get_device("cc2500"))
{
// link to instance for irq_trampoline
radio_instance = this;
}
/*
initialise radio
*/
bool AP_Radio_cc2500::init(void)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
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. */
#endif
sem = hal.util->new_semaphore();
return reset();
}
/*
reset radio
*/
bool AP_Radio_cc2500::reset(void)
{
if (!cc2500.lock_bus()) {
return false;
}
radio_init();
cc2500.unlock_bus();
return true;
}
/*
return statistics structure from radio
*/
const AP_Radio::stats &AP_Radio_cc2500::get_stats(void)
{
return stats;
}
/*
read one pwm channel from radio
*/
uint16_t AP_Radio_cc2500::read(uint8_t chan)
{
if (chan >= CC2500_MAX_CHANNELS) {
return 0;
}
return pwm_channels[chan];
}
/*
update status - called from main thread
*/
void AP_Radio_cc2500::update(void)
{
check_fw_ack();
}
/*
return number of active channels
*/
uint8_t AP_Radio_cc2500::num_channels(void)
{
uint32_t now = AP_HAL::millis();
uint8_t chan = get_rssi_chan();
if (chan > 0) {
pwm_channels[chan-1] = t_status.rssi;
chan_count = MAX(chan_count, chan);
}
chan = get_pps_chan();
if (chan > 0) {
pwm_channels[chan-1] = t_status.pps;
chan_count = MAX(chan_count, chan);
}
chan = get_tx_rssi_chan();
if (chan > 0) {
pwm_channels[chan-1] = tx_rssi;
chan_count = MAX(chan_count, chan);
}
chan = get_tx_pps_chan();
if (chan > 0) {
pwm_channels[chan-1] = tx_pps;
chan_count = MAX(chan_count, chan);
}
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));
}
lost=0;
timeouts=0;
}
return chan_count;
}
/*
return time of last receive in microseconds
*/
uint32_t AP_Radio_cc2500::last_recv_us(void)
{
return packet_timer;
}
/*
send len bytes as a single packet
*/
bool AP_Radio_cc2500::send(const uint8_t *pkt, uint16_t len)
{
// disabled for now
return false;
}
const AP_Radio_cc2500::config AP_Radio_cc2500::radio_config[] = {
{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_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_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_14_MDMCFG0, 0x7A}, // chan spacing 299.926757kHz for 26MHz crystal
{CC2500_15_DEVIATN, 0x51}, // modem deviation 25.128906kHz for 26MHz crystal
{CC2500_19_FOCCFG, 0x16}, // frequency offset compensation
{CC2500_1A_BSCFG, 0x6C}, // bit sync config
{CC2500_1B_AGCCTRL2, 0x43}, // target amplitude 33dB
{CC2500_1C_AGCCTRL1, 0x40}, // AGC control 2
{CC2500_1D_AGCCTRL0, 0x91}, // AGC control 0
{CC2500_21_FREND1, 0x56}, // frontend config1
{CC2500_22_FREND0, 0x10}, // frontend config0
{CC2500_23_FSCAL3, 0xA9}, // frequency synth cal3
{CC2500_24_FSCAL2, 0x0A}, // frequency synth cal2
{CC2500_25_FSCAL1, 0x00}, // frequency synth cal1
{CC2500_26_FSCAL0, 0x11}, // frequency synth cal0
{CC2500_29_FSTEST, 0x59}, // test bits
{CC2500_2C_TEST2, 0x88}, // test settings
{CC2500_2D_TEST1, 0x31}, // test settings
{CC2500_2E_TEST0, 0x0B}, // test settings
{CC2500_03_FIFOTHR, 0x07}, // TX fifo threashold 33, RX fifo threshold 32
{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
};
/*
initialise the radio
*/
void AP_Radio_cc2500::radio_init(void)
{
if (cc2500.ReadReg(CC2500_30_PARTNUM | CC2500_READ_BURST) != 0x80 ||
cc2500.ReadReg(CC2500_31_VERSION | CC2500_READ_BURST) != 0x03) {
Debug(1, "cc2500: radio not found\n");
return;
}
Debug(1, "cc2500: radio_init starting\n");
cc2500.Reset();
hal.scheduler->delay_microseconds(100);
for (uint8_t i=0; i<ARRAY_SIZE(radio_config); i++) {
// write twice to cope with possible SPI errors
cc2500.WriteRegCheck(radio_config[i].reg, radio_config[i].value);
}
cc2500.Strobe(CC2500_SIDLE); // Go to idle...
for (uint8_t c=0;c<0xFF;c++) {
//calibrate all channels
cc2500.Strobe(CC2500_SIDLE);
cc2500.WriteRegCheck(CC2500_0A_CHANNR, c);
cc2500.Strobe(CC2500_SCAL);
hal.scheduler->delay_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_PX4
stm32_gpiosetevent(CYRF_IRQ_INPUT, true, false, false, irq_radio_trampoline);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
hal.gpio->attach_interrupt(HAL_GPIO_RADIO_IRQ, trigger_irq_radio_event, AP_HAL::GPIO::INTERRUPT_RISING);
#endif
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;
}
chVTSet(&timeout_vt, chTimeMS2I(10), trigger_timeout_event, nullptr);
}
void AP_Radio_cc2500::trigger_irq_radio_event()
{
//we are called from ISR context
chSysLockFromISR();
irq_time_us = AP_HAL::micros();
chEvtSignalI(_irq_handler_ctx, EVT_IRQ);
chSysUnlockFromISR();
}
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);
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);
Debug(1,"Starting bind\n");
}
// handle a data96 mavlink packet for fw upload
void AP_Radio_cc2500::handle_data_packet(mavlink_channel_t chan, const mavlink_data96_t &m)
{
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));
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();
}
}
/*
handle a FrSky D16 packet
*/
bool AP_Radio_cc2500::handle_D16_packet(const uint8_t *packet)
{
if (packet[0] != 0x1D) {
return false;
}
if (packet[1] != bindTxId[0] ||
packet[2] != bindTxId[1]) {
Debug(3, "p1=%02x p2=%02x p6=%02x\n", packet[1], packet[2], packet[6]);
// not for us
return false;
}
if (packet[7] == 0x00 ||
packet[7] == 0x20 ||
packet[7] == 0x10 ||
packet[7] == 0x12 ||
packet[7] == 0x14 ||
packet[7] == 0x16 ||
packet[7] == 0x18 ||
packet[7] == 0x1A ||
packet[7] == 0x1C ||
packet[7] == 0x1E) {
// channel packet or range check packet
parse_frSkyX(packet);
packet3 = packet[3];
uint8_t hop_chan = packet[4] & 0x3F;
uint8_t skip = (packet[4]>>6) | (packet[5]<<2);
if (channr != hop_chan) {
Debug(2, "channr=%u hop_chan=%u\n", channr, hop_chan);
}
channr = hop_chan;
if (chanskip != skip) {
Debug(2, "chanskip=%u skip=%u\n", chanskip, skip);
}
chanskip = skip;
return true;
}
return false;
}
/*
handle a SRT packet
*/
bool AP_Radio_cc2500::handle_SRT_packet(const uint8_t *packet)
{
const struct srt_packet *pkt = (const struct srt_packet *)packet;
if (pkt->length != sizeof(struct srt_packet)-1 ||
pkt->txid[0] != bindTxId[0] ||
pkt->txid[1] != bindTxId[1]) {
Debug(3, "len=%u p1=%02x p2=%02x\n", pkt->length, pkt->txid[0], pkt->txid[1]);
// not for us
return false;
}
if (pkt->version != 1) {
// 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);
// 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;
uint8_t data = pkt->data;
/*
decode special data field
*/
switch (pkt->pkt_type) {
case PKTYPE_VOLTAGE:
// voltage from TX is in 0.025 volt units. Convert to 0.01 volt units for easier display
pwm_channels[6] = data * 4;
break;
case PKTYPE_YEAR:
tx_date.firmware_year = data;
break;
case PKTYPE_MONTH:
tx_date.firmware_month = data;
break;
case PKTYPE_DAY:
tx_date.firmware_day = data;
break;
case PKTYPE_TELEM_RSSI:
tx_rssi = data;
break;
case PKTYPE_TELEM_PPS:
tx_pps = data;
break;
case PKTYPE_BL_VERSION:
// unused so far for cc2500
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();
}
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;
}
if (pkt->chanskip != chanskip) {
Debug(2, "chanskip=%u skip=%u\n", chanskip, pkt->chanskip);
chanskip = pkt->chanskip;
}
return true;
}
// main IRQ handler
void AP_Radio_cc2500::irq_handler(void)
{
uint8_t ccLen;
bool matched = false;
do {
ccLen = cc2500.ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST);
hal.scheduler->delay_microseconds(20);
uint8_t ccLen2 = cc2500.ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST);
matched = (ccLen == ccLen2);
} while (!matched);
if (ccLen & 0x80) {
Debug(3,"Fifo overflow %02x\n", ccLen);
// RX FIFO overflow
cc2500.Strobe(CC2500_SFRX);
cc2500.Strobe(CC2500_SRX);
return;
}
uint8_t packet[ccLen];
cc2500.ReadFifo(packet, ccLen);
if (get_fcc_test() != 0) {
// don't process interrupts in FCCTEST mode
return;
}
if (ccLen != 32 && ccLen != sizeof(srt_packet)+2) {
cc2500.Strobe(CC2500_SFRX);
cc2500.Strobe(CC2500_SRX);
Debug(3, "bad len %u\n", ccLen);
return;
}
if (get_debug_level() > 6) {
Debug(6, "CC2500 IRQ state=%u\n", unsigned(protocolState));
Debug(6,"len=%u\n", ccLen);
for (uint8_t i=0; i<ccLen; i++) {
Debug(6, "%02x:%02x ", i, packet[i]);
if ((i+1) % 16 == 0) {
Debug(6, "\n");
}
}
if (ccLen % 16 != 0) {
Debug(6, "\n");
}
}
if (!check_crc(ccLen, packet)) {
Debug(3, "bad CRC ccLen=%u\n", ccLen);
return;
}
switch (protocolState) {
case STATE_BIND_TUNING:
tuneRx(ccLen, packet);
break;
case STATE_BIND_BINDING:
if (getBindData(ccLen, packet)) {
Debug(2,"Bind complete\n");
protocolState = STATE_BIND_COMPLETE;
}
break;
case STATE_BIND_COMPLETE:
protocolState = STATE_STARTING;
save_bind_info();
Debug(3,"listLength=%u\n", listLength);
Debug(3,"Saved bind info\n");
break;
case STATE_STARTING:
listLength = 47;
initialiseData(0);
protocolState = STATE_SEARCH;
chanskip = 1;
nextChannel(1);
break;
case STATE_SEARCH:
protocolState = STATE_DATA;
FALLTHROUGH;
case STATE_DATA: {
bool ok = false;
if (ccLen == 32) {
ok = handle_D16_packet(packet);
} else if (ccLen == sizeof(srt_packet)+2) {
ok = handle_SRT_packet(packet);
}
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;
}
rssi_filtered = 0.95 * rssi_filtered + 0.05 * rssi_dbm;
t_status.rssi = uint8_t(MAX(rssi_filtered, 1));
stats.recv_packets++;
packet_timer = irq_time_us;
chVTSet(&timeout_vt, chTimeMS2I(10), 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();
}
// 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;
}
case STATE_FCCTEST:
// nothing to do, all done in timeout code
Debug(3,"IRQ in FCCTEST state\n");
break;
default:
Debug(2,"state %u\n", (unsigned)protocolState);
break;
}
}
// 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();
}
switch (protocolState) {
case STATE_BIND_TUNING: {
if (bindOffset >= 126) {
if (check_best_LQI()) {
return;
}
bindOffset = -126;
}
uint32_t now = AP_HAL::millis();
if (now - timeTunedMs > 20) {
timeTunedMs = now;
bindOffset += 5;
Debug(6,"bindOffset=%d\n", int(bindOffset));
cc2500.Strobe(CC2500_SIDLE);
cc2500.WriteRegCheck(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
cc2500.Strobe(CC2500_SFRX);
cc2500.Strobe(CC2500_SRX);
}
break;
}
case STATE_DATA: {
uint32_t now = AP_HAL::micros();
if (now - packet_timer > 50*sync_time_us) {
Debug(3,"searching %u\n", unsigned(now - packet_timer));
cc2500.Strobe(CC2500_SIDLE);
cc2500.Strobe(CC2500_SFRX);
nextChannel(1);
cc2500.Strobe(CC2500_SRX);
timeouts++;
protocolState = STATE_SEARCH;
} else {
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);
break;
case STATE_FCCTEST: {
if (get_fcc_test() == 0) {
protocolState = STATE_DATA;
Debug(1,"Ending FCCTEST\n");
}
setChannel(labs(get_fcc_test()) * 10);
cc2500.SetPower(get_transmit_power());
send_D16_telemetry();
break;
}
default:
break;
}
}
void AP_Radio_cc2500::irq_handler_thd(void *arg)
{
(void)arg;
while(true) {
eventmask_t evt = chEvtWaitAny(ALL_EVENTS);
radio_instance->cc2500.lock_bus();
switch(evt) {
case EVT_IRQ:
if (radio_instance->protocolState == STATE_FCCTEST) {
hal.console->printf("IRQ FCC\n");
}
radio_instance->irq_handler();
break;
case EVT_TIMEOUT:
if (radio_instance->cc2500.ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x80) {
irq_time_us = AP_HAL::micros();
radio_instance->irq_handler();
} else {
radio_instance->irq_timeout();
}
break;
case EVT_BIND:
radio_instance->initTuneRx();
break;
default:
break;
}
radio_instance->cc2500.unlock_bus();
}
}
void AP_Radio_cc2500::initTuneRx(void)
{
cc2500.WriteReg(CC2500_19_FOCCFG, 0x14);
timeTunedMs = AP_HAL::millis();
bindOffset = -126;
best_lqi = 255;
best_bindOffset = bindOffset;
cc2500.WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
//cc2500.WriteReg(CC2500_07_PKTCTRL1, 0x0C);
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);
}
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_07_PKTCTRL1, 0x0D); // address check, no broadcast, autoflush, status enable
cc2500.WriteRegCheck(CC2500_19_FOCCFG, 0x16);
hal.scheduler->delay_microseconds(10*1000);
}
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);
hal.scheduler->delay_microseconds(20); // waiting flush FIFO
cc2500.Strobe(CC2500_SRX);
listLength = 0;
}
/*
we've wrapped in the search for the best bind offset. Accept the
best so far if its good enough
*/
bool AP_Radio_cc2500::check_best_LQI(void)
{
if (best_lqi >= 50) {
return false;
}
bindOffset = best_bindOffset;
initGetBind();
initialiseData(1);
protocolState = STATE_BIND_BINDING;
bind_mask = 0;
listLength = 0;
Debug(2,"Bind tuning %d with Lqi %u\n", best_bindOffset, best_lqi);
return true;
}
/*
check if we have received a packet with sufficiently good link
quality to start binding
*/
bool AP_Radio_cc2500::tuneRx(uint8_t ccLen, uint8_t *packet)
{
if (bindOffset >= 126) {
// we've scanned the whole range, if any were below 50 then
// accept it
if (check_best_LQI()) {
return true;
}
bindOffset = -126;
}
if ((packet[ccLen - 1] & 0x80) && packet[2] == 0x01) {
uint8_t Lqi = packet[ccLen - 1] & 0x7F;
if (Lqi < best_lqi) {
best_lqi = Lqi;
best_bindOffset = bindOffset;
}
}
return false;
}
/*
get a block of hopping data from a bind packet
*/
bool AP_Radio_cc2500::getBindData(uint8_t ccLen, uint8_t *packet)
{
// parse a bind data packet */
if ((packet[ccLen - 1] & 0x80) && packet[2] == 0x01) {
if (bind_mask == 0) {
bindTxId[0] = packet[3];
bindTxId[1] = packet[4];
} else if (bindTxId[0] != packet[3] ||
bindTxId[1] != packet[4]) {
Debug(2,"Bind restart\n");
bind_mask = 0;
listLength = 0;
}
for (uint8_t n = 0; n < 5; n++) {
uint8_t c = packet[5] + n;
if (c < sizeof(bindHopData)) {
bindHopData[c] = packet[6 + n];
bind_mask |= (uint64_t(1)<<c);
listLength = MAX(listLength, c+1);
}
}
// bind has finished when we have hopping data for all channels
return (listLength == 47 && bind_mask == ((uint64_t(1)<<47)-1));
}
return false;
}
void AP_Radio_cc2500::setChannel(uint8_t channel)
{
cc2500.Strobe(CC2500_SIDLE);
cc2500.WriteReg(CC2500_23_FSCAL3, calData[channel][0]);
cc2500.WriteReg(CC2500_24_FSCAL2, calData[channel][1]);
cc2500.WriteReg(CC2500_25_FSCAL1, calData[channel][2]);
cc2500.WriteReg(CC2500_0A_CHANNR, channel);
cc2500.Strobe(CC2500_SRX);
}
void AP_Radio_cc2500::nextChannel(uint8_t skip)
{
channr = (channr + skip) % listLength;
setChannel(bindHopData[channr]);
}
void AP_Radio_cc2500::parse_frSkyX(const uint8_t *packet)
{
uint16_t c[8];
c[0] = (uint16_t)((packet[10] <<8)& 0xF00) | packet[9];
c[1] = (uint16_t)((packet[11]<<4)&0xFF0) | (packet[10]>>4);
c[2] = (uint16_t)((packet[13] <<8)& 0xF00) | packet[12];
c[3] = (uint16_t)((packet[14]<<4)&0xFF0) | (packet[13]>>4);
c[4] = (uint16_t)((packet[16] <<8)& 0xF00) | packet[15];
c[5] = (uint16_t)((packet[17]<<4)&0xFF0) | (packet[16]>>4);
c[6] = (uint16_t)((packet[19] <<8)& 0xF00) | packet[18];
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) {
j = 8;
c[i] = c[i] - 2048;
} else {
j = 0;
}
if (c[i] == 0) {
continue;
}
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) {
pwm_channels[chan] = word_temp;
if (chan >= chan_count) {
chan_count = chan+1;
}
}
}
}
}
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++) {
crc = (crc<<8) ^ (CRCTable[((uint8_t)(crc>>8) ^ *data++) & 0xFF]);
}
return crc;
}
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]);
} else if (ccLen == 32) {
// D16 packet
uint16_t lcrc = calc_crc(&packet[3],(ccLen-7));
return ((lcrc >>8)==packet[ccLen-4] && (lcrc&0x00FF)==packet[ccLen-3]);
}
return false;
}
/*
save bind info
*/
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;
memcpy(info.bindHopData, bindHopData, sizeof(info.bindHopData));
bind_storage.write_block(0, &info, sizeof(info));
}
/*
load bind info
*/
bool AP_Radio_cc2500::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;
}
bindTxId[0] = info.bindTxId[0];
bindTxId[1] = info.bindTxId[1];
bindOffset = info.bindOffset;
listLength = info.listLength;
memcpy(bindHopData, info.bindHopData, sizeof(bindHopData));
return true;
}
/*
send a D16 telemetry packet
*/
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];
frame[3] = packet3;
if (telem_send_rssi) {
frame[4] = MAX(MIN(t_status.rssi, 0x7f),1) | 0x80;
} else {
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
cc2500.WriteFifo(frame, sizeof(frame));
}
cc2500.Strobe(CC2500_STX);
}
/*
send a SRT telemetry packet
*/
void AP_Radio_cc2500::send_SRT_telemetry(void)
{
struct telem_packet_cc2500 pkt {};
pkt.length = sizeof(pkt)-1;
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();
} else {
pkt.type = TELEM_STATUS;
pkt.payload.status = t_status;
}
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
// continuous transmission
cc2500.WriteFifo((const uint8_t *)&pkt, sizeof(pkt));
}
cc2500.Strobe(CC2500_STX);
}
/*
send a fwupload ack if needed
*/
void AP_Radio_cc2500::check_fw_ack(void)
{
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();
}
}
#endif // HAL_RCINPUT_WITH_AP_RADIO
#endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
Reference in New Issue View Git Blame Copy Permalink