rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-10 18:24:11 +01:00
Code Issues Releases Activity
246706ac9d
LibrePilot/flight/pios/common/pios_openlrs.c

1514 lines
56 KiB
C
Raw Blame History

/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_RFM22B Radio Functions
* @brief PIOS OpenLRS interface for for the RFM22B radio
* @{
*
* @file pios_openlrs.c
* @author Tau Labs, http://taulabs.org, Copyright (C) 2015
* @author dRonin, http://dronin.org Copyright (C) 2015
* @author LibrePilot, http://librepilot.org Copyright (C) 2016
* @brief Implements an OpenLRS driver for the RFM22B
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* 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, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#ifdef PIOS_INCLUDE_OPENLRS
#include <pios_spi_priv.h>
#include <pios_openlrs_priv.h>
#include <pios_openlrs_rcvr_priv.h>
#include <pios_semaphore.h>
#include <pios_thread.h>
#include <taskinfo.h>
#include "flightstatus.h"
#include "flightbatterystate.h"
#include "oplinksettings.h"
#include "oplinkstatus.h"
#include "pios_rfm22b_regs.h"
#define STACK_SIZE_BYTES 800
#define TASK_PRIORITY PIOS_THREAD_PRIO_NORMAL
static void rx_reset(struct pios_openlrs_dev *openlrs_dev);
static void rfmSetCarrierFrequency(struct pios_openlrs_dev *openlrs_dev, uint32_t f);
static uint8_t rfmGetRSSI(struct pios_openlrs_dev *openlrs_dev);
static void to_rx_mode(struct pios_openlrs_dev *openlrs_dev);
static void tx_packet(struct pios_openlrs_dev *openlrs_dev, uint8_t *pkt, uint8_t size);
static struct pios_openlrs_dev *pios_openlrs_alloc();
static bool pios_openlrs_validate(struct pios_openlrs_dev *openlrs_dev);
// SPI read/write functions
static void rfm22_assertCs(struct pios_openlrs_dev *openlrs_dev);
static void rfm22_deassertCs(struct pios_openlrs_dev *openlrs_dev);
static void rfm22_claimBus(struct pios_openlrs_dev *openlrs_dev);
static void rfm22_releaseBus(struct pios_openlrs_dev *openlrs_dev);
static void rfm22_write_claim(struct pios_openlrs_dev *openlrs_dev,
uint8_t addr, uint8_t data);
static void rfm22_write(struct pios_openlrs_dev *openlrs_dev, uint8_t addr,
uint8_t data);
static uint8_t rfm22_read_claim(struct pios_openlrs_dev *openlrs_dev,
uint8_t addr);
static uint8_t rfm22_read(struct pios_openlrs_dev *openlrs_dev,
uint8_t addr);
// Private constants
const struct rfm22_modem_regs {
uint32_t bps;
uint8_t r_1c, r_1d, r_1e, r_20, r_21, r_22, r_23, r_24, r_25, r_2a, r_6e, r_6f, r_70, r_71, r_72;
} modem_params[] = {
{ 4800, 0x1a, 0x40, 0x0a, 0xa1, 0x20, 0x4e, 0xa5, 0x00, 0x1b, 0x1e, 0x27, 0x52, 0x2c, 0x23, 0x30 }, // 50000 0x00
{ 9600, 0x05, 0x40, 0x0a, 0xa1, 0x20, 0x4e, 0xa5, 0x00, 0x20, 0x24, 0x4e, 0xa5, 0x2c, 0x23, 0x30 }, // 25000 0x00
{ 19200, 0x06, 0x40, 0x0a, 0xd0, 0x00, 0x9d, 0x49, 0x00, 0x7b, 0x28, 0x9d, 0x49, 0x2c, 0x23, 0x30 }, // 25000 0x01
{ 57600, 0x05, 0x40, 0x0a, 0x45, 0x01, 0xd7, 0xdc, 0x03, 0xb8, 0x1e, 0x0e, 0xbf, 0x00, 0x23, 0x2e },
{ 125000, 0x8a, 0x40, 0x0a, 0x60, 0x01, 0x55, 0x55, 0x02, 0xad, 0x1e, 0x20, 0x00, 0x00, 0x23, 0xc8 },
};
static const uint8_t pktsizes[8] = { 0, 7, 11, 12, 16, 17, 21, 0 };
static const uint8_t OUT_FF[64] = {
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
};
const uint8_t default_hop_list[] = { DEFAULT_HOPLIST };
const uint32_t packet_advance_time_us = 1500;
const uint32_t packet_timeout_us = 1000;
const struct rfm22_modem_regs bind_params =
{ 9600, 0x05, 0x40, 0x0a, 0xa1, 0x20, 0x4e, 0xa5, 0x00, 0x20, 0x24, 0x4e, 0xa5, 0x2c, 0x23, 0x30 };
static uint32_t minFreq()
{
return MIN_RFM_FREQUENCY_433;
}
static uint32_t maxFreq()
{
return MAX_RFM_FREQUENCY_433;
}
static uint32_t defCarrierFreq()
{
return DEFAULT_CARRIER_FREQUENCY_433;
}
static uint32_t bindingFreq()
{
return BINDING_FREQUENCY_433;
}
/*****************************************************************************
* OpenLRS data formatting utilities
*****************************************************************************/
static uint8_t getPacketSize(struct bind_data *bd)
{
return pktsizes[(bd->flags & 0x07)];
}
static uint32_t getInterval(struct bind_data *bd)
{
uint32_t ret;
// Sending a x byte packet on bps y takes about (emperical)
// usec = (x + 15) * 8200000 / baudrate
#define BYTES_AT_BAUD_TO_USEC(bytes, bps, div) ((uint32_t)((bytes) + (div ? 20 : 15)) * 8200000L / (uint32_t)(bps))
ret = (BYTES_AT_BAUD_TO_USEC(getPacketSize(bd), modem_params[bd->modem_params].bps, bd->flags & DIVERSITY_ENABLED) + 2000);
if (bd->flags & TELEMETRY_MASK) {
ret += (BYTES_AT_BAUD_TO_USEC(TELEMETRY_PACKETSIZE, modem_params[bd->modem_params].bps, bd->flags & DIVERSITY_ENABLED) + 1000);
}
// round up to ms
ret = ((ret + 999) / 1000) * 1000;
// enable following to limit packet rate to 50Hz at most
#ifdef LIMIT_RATE_TO_50HZ
if (ret < 20000) {
ret = 20000;
}
#endif
return ret;
}
static void unpackChannels(uint8_t config, int16_t PPM[], uint8_t *p)
{
uint8_t i;
for (i = 0; i <= (config / 2); i++) { // 4ch packed in 5 bytes
PPM[0] = (((uint16_t)p[4] & 0x03) << 8) + p[0];
PPM[1] = (((uint16_t)p[4] & 0x0c) << 6) + p[1];
PPM[2] = (((uint16_t)p[4] & 0x30) << 4) + p[2];
PPM[3] = (((uint16_t)p[4] & 0xc0) << 2) + p[3];
p += 5;
PPM += 4;
}
if (config & 1) { // 4ch packed in 1 byte;
PPM[0] = (((uint16_t)p[0] >> 6) & 3) * 333 + 12;
PPM[1] = (((uint16_t)p[0] >> 4) & 3) * 333 + 12;
PPM[2] = (((uint16_t)p[0] >> 2) & 3) * 333 + 12;
PPM[3] = (((uint16_t)p[0] >> 0) & 3) * 333 + 12;
}
}
// ! Apply the OpenLRS rescaling to the channels
static void rescaleChannels(int16_t PPM[])
{
for (uint32_t i = 0; i < OPENLRS_PPM_NUM_CHANNELS; i++) {
int16_t x = PPM[i];
int16_t ret;
if (x < 12) {
ret = 808 + x * 16;
} else if (x < 1012) {
ret = x + 988;
} else if (x < 1024) {
ret = 2000 + (x - 1011) * 16;
} else {
ret = 2192;
}
PPM[i] = ret;
}
}
static uint8_t countSetBits(uint16_t x)
{
x = x - ((x >> 1) & 0x5555);
x = (x & 0x3333) + ((x >> 2) & 0x3333);
x = x + (x >> 4);
x &= 0x0F0F;
return (x * 0x0101) >> 8;
}
/*****************************************************************************
* OpenLRS hardware access
*****************************************************************************/
#define NOP() __asm__ __volatile__ ("nop")
#define RF22B_PWRSTATE_POWERDOWN 0x00
#define RF22B_PWRSTATE_READY RFM22_opfc1_xton
#define RF22B_PWRSTATE_RX (RFM22_opfc1_rxon | RFM22_opfc1_xton)
#define RF22B_PWRSTATE_TX (RFM22_opfc1_txon | RFM22_opfc1_xton)
#define RF22B_PACKET_SENT_INTERRUPT RFM22_ie1_enpksent
#define RF22B_RX_PACKET_RECEIVED_IRQ RFM22_ie1_enpkvalid
static void rfmSetChannel(struct pios_openlrs_dev *openlrs_dev, uint8_t ch)
{
// DEBUG_PRINTF(3,"rfmSetChannel %d\r\n", ch);
uint8_t magicLSB = (openlrs_dev->bind_data.rf_magic & 0xff) ^ ch;
rfm22_claimBus(openlrs_dev);
rfm22_write(openlrs_dev, RFM22_frequency_hopping_channel_select, openlrs_dev->bind_data.hopchannel[ch]);
rfm22_write(openlrs_dev, RFM22_transmit_header3 + 3, magicLSB);
rfm22_write(openlrs_dev, RFM22_check_header3 + 3, magicLSB);
rfm22_releaseBus(openlrs_dev);
}
static uint8_t rfmGetRSSI(struct pios_openlrs_dev *openlrs_dev)
{
return rfm22_read_claim(openlrs_dev, 0x26);
}
static uint16_t rfmGetAFCC(struct pios_openlrs_dev *openlrs_dev)
{
return ((uint16_t)rfm22_read_claim(openlrs_dev, 0x2B) << 2) | ((uint16_t)rfm22_read_claim(openlrs_dev, 0x2C) >> 6);
}
static void setModemRegs(struct pios_openlrs_dev *openlrs_dev, const struct rfm22_modem_regs *r)
{
DEBUG_PRINTF(3, "setModemRegs\r\n");
rfm22_claimBus(openlrs_dev);
rfm22_write(openlrs_dev, RFM22_if_filter_bandwidth, r->r_1c);
rfm22_write(openlrs_dev, RFM22_afc_loop_gearshift_override, r->r_1d);
rfm22_write(openlrs_dev, RFM22_afc_timing_control, r->r_1e);
rfm22_write(openlrs_dev, RFM22_clk_recovery_oversampling_ratio, r->r_20);
rfm22_write(openlrs_dev, RFM22_clk_recovery_offset2, r->r_21);
rfm22_write(openlrs_dev, RFM22_clk_recovery_offset1, r->r_22);
rfm22_write(openlrs_dev, RFM22_clk_recovery_offset0, r->r_23);
rfm22_write(openlrs_dev, RFM22_clk_recovery_timing_loop_gain1, r->r_24);
rfm22_write(openlrs_dev, RFM22_clk_recovery_timing_loop_gain0, r->r_25);
rfm22_write(openlrs_dev, RFM22_afc_limiter, r->r_2a);
rfm22_write(openlrs_dev, RFM22_tx_data_rate1, r->r_6e);
rfm22_write(openlrs_dev, RFM22_tx_data_rate0, r->r_6f);
rfm22_write(openlrs_dev, RFM22_modulation_mode_control1, r->r_70);
rfm22_write(openlrs_dev, RFM22_modulation_mode_control2, r->r_71);
rfm22_write(openlrs_dev, RFM22_frequency_deviation, r->r_72);
rfm22_releaseBus(openlrs_dev);
}
static void rfmSetCarrierFrequency(struct pios_openlrs_dev *openlrs_dev, uint32_t f)
{
/* Protect ourselves from out-of-band frequencies. Ideally we'd latch
* an error here and prevent tx, but this is good enough to protect
* the hardware. */
if ((f < minFreq(openlrs_dev->band)) ||
(f > maxFreq(openlrs_dev->band))) {
f = defCarrierFreq(openlrs_dev->band);
}
// DEBUG_PRINTF(3,"rfmSetCarrierFrequency %d\r\n", f);
uint16_t fb, fc, hbsel;
if (f < 480000000) {
hbsel = 0;
fb = f / 10000000 - 24;
fc = (f - (fb + 24) * 10000000) * 4 / 625;
} else {
hbsel = 1;
fb = f / 20000000 - 24;
fc = (f - (fb + 24) * 20000000) * 2 / 625;
}
rfm22_claimBus(openlrs_dev);
rfm22_write(openlrs_dev, RFM22_frequency_band_select, RFM22_fbs_sbse + (hbsel ? RFM22_fbs_hbsel : 0) + (fb & RFM22_fb_mask));
rfm22_write(openlrs_dev, RFM22_nominal_carrier_frequency1, (fc >> 8));
rfm22_write(openlrs_dev, RFM22_nominal_carrier_frequency0, (fc & 0xff));
rfm22_releaseBus(openlrs_dev);
}
static void init_rfm(struct pios_openlrs_dev *openlrs_dev, uint8_t isbind)
{
DEBUG_PRINTF(2, "init_rfm %d\r\n", isbind);
if (!isbind) {
DEBUG_PRINTF(2, "Binding settings:\r\n");
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " version: %d\r\n", openlrs_dev->bind_data.version);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " serial_baudrate: %d\r\n", openlrs_dev->bind_data.serial_baudrate);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " rf_frequency: %d\r\n", openlrs_dev->bind_data.rf_frequency);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " rf_power: %d\r\n", openlrs_dev->bind_data.rf_power);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " rf_channel_spacing: %d\r\n", openlrs_dev->bind_data.rf_channel_spacing);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " modem_params: %d\r\n", openlrs_dev->bind_data.modem_params);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " flags: %d\r\n", openlrs_dev->bind_data.flags);
PIOS_Thread_Sleep(10);
}
rfm22_claimBus(openlrs_dev);
openlrs_dev->it_status1 = rfm22_read(openlrs_dev, RFM22_interrupt_status1); // read status, clear interrupt
openlrs_dev->it_status2 = rfm22_read(openlrs_dev, RFM22_interrupt_status2);
rfm22_write(openlrs_dev, RFM22_interrupt_enable2, 0x00); // disable interrupts
rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl1, RF22B_PWRSTATE_READY); // disable lbd, wakeup timer, use internal 32768,xton = 1; in ready mode
rfm22_write(openlrs_dev, RFM22_xtal_osc_load_cap, 0x7f); // c = 12.5p
rfm22_write(openlrs_dev, RFM22_cpu_output_clk, 0x05);
switch (openlrs_dev->cfg.gpio_direction) {
case GPIO0_TX_GPIO1_RX:
rfm22_write(openlrs_dev, RFM22_gpio0_config, RFM22_gpio0_config_txstate); // gpio0 TX State
rfm22_write(openlrs_dev, RFM22_gpio1_config, RFM22_gpio1_config_rxstate); // gpio1 RX State
break;
case GPIO0_RX_GPIO1_TX:
rfm22_write(openlrs_dev, RFM22_gpio0_config, RFM22_gpio0_config_rxstate); // gpio0 RX State
rfm22_write(openlrs_dev, RFM22_gpio1_config, RFM22_gpio1_config_txstate); // gpio1 TX State
break;
}
rfm22_write(openlrs_dev, RFM22_gpio2_config, 0xfd); // gpio 2 VDD
rfm22_write(openlrs_dev, RFM22_io_port_config, RFM22_io_port_default); // gpio 0, 1,2 NO OTHER FUNCTION.
rfm22_releaseBus(openlrs_dev);
if (isbind) {
setModemRegs(openlrs_dev, &bind_params);
} else {
setModemRegs(openlrs_dev, &modem_params[openlrs_dev->bind_data.modem_params]);
}
// Packet settings
rfm22_claimBus(openlrs_dev);
rfm22_write(openlrs_dev, RFM22_data_access_control, 0x8c); // enable packet handler, msb first, enable crc,
rfm22_write(openlrs_dev, RFM22_header_control1, 0x0f); // no broadcast, check header bytes 3,2,1,0
rfm22_write(openlrs_dev, RFM22_header_control2, 0x42); // 4 byte header, 2 byte synch, variable pkt size
rfm22_write(openlrs_dev, RFM22_preamble_length, (openlrs_dev->bind_data.flags & DIVERSITY_ENABLED) ? 0x14 : 0x0a); // 40 bit preamble, 80 with diversity
rfm22_write(openlrs_dev, RFM22_preamble_detection_ctrl1, 0x2a); // preath = 5 (20bits), rssioff = 2
rfm22_write(openlrs_dev, RFM22_sync_word3, 0x2d); // synchronize word 3
rfm22_write(openlrs_dev, RFM22_sync_word2, 0xd4); // synchronize word 2
rfm22_write(openlrs_dev, RFM22_sync_word1, 0x00); // synch word 1 (not used)
rfm22_write(openlrs_dev, RFM22_sync_word0, 0x00); // synch word 0 (not used)
uint32_t magic = isbind ? BIND_MAGIC : openlrs_dev->bind_data.rf_magic;
for (uint8_t i = 0; i < 4; i++) {
rfm22_write(openlrs_dev, RFM22_transmit_header3 + i, (magic >> 24) & 0xff); // tx header
rfm22_write(openlrs_dev, RFM22_check_header3 + i, (magic >> 24) & 0xff); // rx header
magic = magic << 8; // advance to next byte
}
rfm22_write(openlrs_dev, RFM22_header_enable3, 0xff); // all the bit to be checked
rfm22_write(openlrs_dev, RFM22_header_enable2, 0xff); // all the bit to be checked
rfm22_write(openlrs_dev, RFM22_header_enable1, 0xff); // all the bit to be checked
rfm22_write(openlrs_dev, RFM22_header_enable0, 0xff); // all the bit to be checked
if (isbind) {
rfm22_write(openlrs_dev, RFM22_tx_power, BINDING_POWER);
} else {
rfm22_write(openlrs_dev, RFM22_tx_power, openlrs_dev->bind_data.rf_power);
}
rfm22_write(openlrs_dev, RFM22_frequency_hopping_channel_select, 0);
rfm22_write(openlrs_dev, RFM22_frequency_hopping_step_size, openlrs_dev->bind_data.rf_channel_spacing); // channel spacing
rfm22_write(openlrs_dev, RFM22_frequency_offset1, 0x00);
rfm22_write(openlrs_dev, RFM22_frequency_offset2, 0x00); // no offset
rfm22_releaseBus(openlrs_dev);
rfmSetCarrierFrequency(openlrs_dev, isbind ? bindingFreq(openlrs_dev->band) : openlrs_dev->bind_data.rf_frequency);
}
static void to_rx_mode(struct pios_openlrs_dev *openlrs_dev)
{
// DEBUG_PRINTF(3,"to_rx_mode\r\n");
rfm22_claimBus(openlrs_dev);
openlrs_dev->it_status1 = rfm22_read(openlrs_dev, RFM22_interrupt_status1);
openlrs_dev->it_status2 = rfm22_read(openlrs_dev, RFM22_interrupt_status2);
rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl1, RF22B_PWRSTATE_READY);
rfm22_releaseBus(openlrs_dev);
PIOS_Thread_Sleep(10);
rx_reset(openlrs_dev);
NOP();
}
static void clearFIFO(struct pios_openlrs_dev *openlrs_dev)
{
// DEBUG_PRINTF(3,"clearFIFO\r\n");
rfm22_claimBus(openlrs_dev);
rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl2, 0x03);
rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl2, 0x00);
rfm22_releaseBus(openlrs_dev);
}
static void rx_reset(struct pios_openlrs_dev *openlrs_dev)
{
// DEBUG_PRINTF(3,"rx_reset\r\n");
rfm22_write_claim(openlrs_dev, RFM22_op_and_func_ctrl1, RF22B_PWRSTATE_READY);
rfm22_write_claim(openlrs_dev, RFM22_rx_fifo_control, 36); // threshold for rx almost full, interrupt when 1 byte received
clearFIFO(openlrs_dev);
rfm22_claimBus(openlrs_dev);
rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl1, RF22B_PWRSTATE_RX); // to rx mode
rfm22_write(openlrs_dev, RFM22_interrupt_enable1, RF22B_RX_PACKET_RECEIVED_IRQ);
openlrs_dev->it_status1 = rfm22_read(openlrs_dev, RFM22_interrupt_status1); // read the Interrupt Status1 register
openlrs_dev->it_status2 = rfm22_read(openlrs_dev, RFM22_interrupt_status2);
rfm22_releaseBus(openlrs_dev);
}
// TODO: move into dev structure
uint32_t tx_start = 0;
static void tx_packet_async(struct pios_openlrs_dev *openlrs_dev, uint8_t *pkt, uint8_t size)
{
rfm22_claimBus(openlrs_dev);
rfm22_write(openlrs_dev, RFM22_transmit_packet_length, size); // total tx size
for (uint8_t i = 0; i < size; i++) {
rfm22_write(openlrs_dev, RFM22_fifo_access, pkt[i]);
}
rfm22_write(openlrs_dev, RFM22_interrupt_enable1, RF22B_PACKET_SENT_INTERRUPT);
openlrs_dev->it_status1 = rfm22_read(openlrs_dev, RFM22_interrupt_status1); // read the Interrupt Status1 register
openlrs_dev->it_status2 = rfm22_read(openlrs_dev, RFM22_interrupt_status2);
tx_start = PIOS_DELAY_GetuS();
rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl1, RF22B_PWRSTATE_TX); // to tx mode
rfm22_releaseBus(openlrs_dev);
openlrs_dev->rf_mode = Transmit;
}
static void tx_packet(struct pios_openlrs_dev *openlrs_dev, uint8_t *pkt, uint8_t size)
{
tx_packet_async(openlrs_dev, pkt, size);
PIOS_Semaphore_Take(openlrs_dev->sema_isr, 25);
#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
// Update the watchdog timer
PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
#endif /* PIOS_WDG_RFM22B */
if (openlrs_dev->rf_mode == Transmit) {
DEBUG_PRINTF(2, "OLRS ERR: tx_packet timeout\r\n");
init_rfm(openlrs_dev, false); // reset modem
}
}
static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_t len __attribute__((unused))) // duration is now in half seconds.
{
DEBUG_PRINTF(2, "beacon_tone: %d %d\r\n", hz, len * 2);
int16_t d = 500000 / hz; // better resolution
#if defined(PIOS_LED_LINK)
PIOS_LED_On(PIOS_LED_LINK);
#endif /* PIOS_LED_LINK */
if (d < 1) {
d = 1;
}
rfm22_claimBus(openlrs_dev);
// This need fixed for F1
#ifdef GPIO_Mode_OUT
GPIO_TypeDef *gpio = openlrs_dev->cfg.spi_cfg->mosi.gpio;
uint16_t pin_source = openlrs_dev->cfg.spi_cfg->mosi.init.GPIO_Pin;
uint8_t remap = openlrs_dev->cfg.spi_cfg->remap;
GPIO_InitTypeDef init = {
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
};
init.GPIO_Pin = pin_source;
// Set MOSI to digital out for bit banging
GPIO_PinAFConfig(gpio, pin_source, 0);
GPIO_Init(gpio, &init);
uint32_t raw_time = PIOS_DELAY_GetRaw();
int16_t cycles = (len * 500000 / d);
for (int16_t i = 0; i < cycles; i++) {
GPIO_SetBits(gpio, pin_source);
PIOS_DELAY_WaituS(d);
GPIO_ResetBits(gpio, pin_source);
PIOS_DELAY_WaituS(d);
// Make sure to give other tasks time to do things
if (PIOS_DELAY_DiffuS(raw_time) > 10000) {
PIOS_Thread_Sleep(1);
raw_time = PIOS_DELAY_GetRaw();
}
}
GPIO_Init(gpio, (GPIO_InitTypeDef *)&openlrs_dev->cfg.spi_cfg->mosi.init);
GPIO_PinAFConfig(gpio, pin_source, remap);
#endif /* ifdef GPIO_Mode_OUT */
rfm22_releaseBus(openlrs_dev);
#if defined(PIOS_LED_LINK)
PIOS_LED_Off(PIOS_LED_LINK);
#endif /* PIOS_LED_LINK */
#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
// Update the watchdog timer
PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
#endif /* PIOS_WDG_RFM22B */
}
static uint8_t beaconGetRSSI(struct pios_openlrs_dev *openlrs_dev)
{
uint16_t rssiSUM = 0;
rfmSetCarrierFrequency(openlrs_dev, openlrs_dev->beacon_frequency);
rfm22_write_claim(openlrs_dev, RFM22_frequency_hopping_channel_select, 0); // ch 0 to avoid offset
PIOS_Thread_Sleep(1);
rssiSUM += rfmGetRSSI(openlrs_dev);
PIOS_Thread_Sleep(1);
rssiSUM += rfmGetRSSI(openlrs_dev);
PIOS_Thread_Sleep(1);
rssiSUM += rfmGetRSSI(openlrs_dev);
PIOS_Thread_Sleep(1);
rssiSUM += rfmGetRSSI(openlrs_dev);
return rssiSUM >> 2;
}
static void beacon_send(struct pios_openlrs_dev *openlrs_dev, bool static_tone)
{
DEBUG_PRINTF(2, "beacon_send\r\n");
rfm22_claimBus(openlrs_dev);
openlrs_dev->it_status1 = rfm22_read(openlrs_dev, 0x03); // read status, clear interrupt
openlrs_dev->it_status2 = rfm22_read(openlrs_dev, 0x04);
rfm22_write(openlrs_dev, 0x06, 0x00); // no wakeup up, lbd,
rfm22_write(openlrs_dev, 0x07, RF22B_PWRSTATE_READY); // disable lbd, wakeup timer, use internal 32768,xton = 1; in ready mode
rfm22_write(openlrs_dev, 0x09, 0x7f); // (default) c = 12.5p
rfm22_write(openlrs_dev, 0x0a, 0x05);
rfm22_write(openlrs_dev, 0x0b, 0x12); // gpio0 TX State
rfm22_write(openlrs_dev, 0x0c, 0x15); // gpio1 RX State
rfm22_write(openlrs_dev, 0x0d, 0xfd); // gpio 2 micro-controller clk output
rfm22_write(openlrs_dev, 0x0e, 0x00); // gpio 0, 1,2 NO OTHER FUNCTION.
rfm22_write(openlrs_dev, 0x70, 0x2C); // disable manchest
rfm22_write(openlrs_dev, 0x30, 0x00); // disable packet handling
rfm22_write(openlrs_dev, 0x79, 0); // start channel
rfm22_write(openlrs_dev, 0x7a, 0x05); // 50khz step size (10khz x value) // no hopping
rfm22_write(openlrs_dev, 0x71, 0x12); // trclk=[00] no clock, dtmod=[01] direct using SPI, fd8=0 eninv=0 modtyp=[10] FSK
rfm22_write(openlrs_dev, 0x72, 0x02); // fd (frequency deviation) 2*625Hz == 1.25kHz
rfm22_write(openlrs_dev, 0x73, 0x00);
rfm22_write(openlrs_dev, 0x74, 0x00); // no offset
rfm22_releaseBus(openlrs_dev);
rfmSetCarrierFrequency(openlrs_dev, openlrs_dev->beacon_frequency);
rfm22_write_claim(openlrs_dev, 0x6d, 0x07); // 7 set max power 100mW
PIOS_Thread_Sleep(10);
rfm22_write_claim(openlrs_dev, 0x07, RF22B_PWRSTATE_TX); // to tx mode
PIOS_Thread_Sleep(10);
if (static_tone) {
uint8_t i = 0;
while (i++ < 20) {
beacon_tone(openlrs_dev, 440, 1);
}
} else {
// close encounters tune
// G, A, F, F(lower octave), C
// octave 3: 392 440 349 175 261
beacon_tone(openlrs_dev, 392, 1);
rfm22_write(openlrs_dev, 0x6d, 0x05); // 5 set mid power 25mW
PIOS_Thread_Sleep(10);
beacon_tone(openlrs_dev, 440, 1);
rfm22_write(openlrs_dev, 0x6d, 0x04); // 4 set mid power 13mW
PIOS_Thread_Sleep(10);
beacon_tone(openlrs_dev, 349, 1);
rfm22_write(openlrs_dev, 0x6d, 0x02); // 2 set min power 3mW
PIOS_Thread_Sleep(10);
beacon_tone(openlrs_dev, 175, 1);
rfm22_write(openlrs_dev, 0x6d, 0x00); // 0 set min power 1.3mW
PIOS_Thread_Sleep(10);
beacon_tone(openlrs_dev, 261, 2);
}
rfm22_write_claim(openlrs_dev, 0x07, RF22B_PWRSTATE_READY);
}
/*****************************************************************************
* High level OpenLRS functions
*****************************************************************************/
// TODO: these should move into device structure, or deleted
// if not useful to be reported via GCS
#define ntohl(v) \
( \
(((v) & 0xFF000000) >> 24) | \
(((v) & 0x00FF0000) >> 8) | \
(((v) & 0x0000FF00) << 8) | \
(((v) & 0x000000FF) << 24))
static uint8_t pios_openlrs_bind_receive(struct pios_openlrs_dev *openlrs_dev, uint32_t timeout)
{
uint32_t start = PIOS_Thread_Systime();
uint8_t rxb;
init_rfm(openlrs_dev, true);
// TODO: move openlrs_dev->rf_mode into dev structure
openlrs_dev->rf_mode = Receive;
to_rx_mode(openlrs_dev);
DEBUG_PRINTF(2, "Waiting bind\r\n");
DEBUG_PRINTF(2, "timeout: %d\r\n", timeout);
DEBUG_PRINTF(2, "start: %d\r\n", start);
uint32_t i = 0;
while ((!timeout) || ((PIOS_Thread_Systime() - start) < timeout)) {
PIOS_Thread_Sleep(1);
#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
// Update the watchdog timer
PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
#endif /* PIOS_WDG_RFM22B */
if (i++ % 100 == 0) {
// DEBUG_PRINTF(2,"Waiting bind\r\n");
#if defined(PIOS_LED_LINK)
PIOS_LED_Toggle(PIOS_LED_LINK);
#endif /* PIOS_LED_LINK */
}
if (openlrs_dev->rf_mode == Received) {
DEBUG_PRINTF(2, "Got pkt\r\n");
// TODO: parse data packet (write command for that)
rfm22_claimBus(openlrs_dev);
rfm22_assertCs(openlrs_dev);
PIOS_SPI_TransferByte(openlrs_dev->spi_id, 0x7f);
rxb = PIOS_SPI_TransferByte(openlrs_dev->spi_id, 0x00);
if (rxb == 'b') {
PIOS_SPI_TransferBlock(openlrs_dev->spi_id, OUT_FF,
(uint8_t *)&openlrs_dev->bind_data, sizeof(struct bind_data), NULL);
rfm22_deassertCs(openlrs_dev);
rfm22_releaseBus(openlrs_dev);
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
if (2 <= DEBUG_LEVEL && pios_com_debug_id > 0) {
DEBUG_PRINTF(2, "Binding settings:\r\n");
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " version: %d\r\n", openlrs_dev->bind_data.version);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " serial_baudrate: %d\r\n", openlrs_dev->bind_data.serial_baudrate);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " rf_frequency: %d\r\n", openlrs_dev->bind_data.rf_frequency);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " rf_power: %d\r\n", openlrs_dev->bind_data.rf_power);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " rf_channel_spacing: %d\r\n", openlrs_dev->bind_data.rf_channel_spacing);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " modem_params: %d\r\n", openlrs_dev->bind_data.modem_params);
PIOS_Thread_Sleep(10);
DEBUG_PRINTF(2, " flags: %d\r\n", openlrs_dev->bind_data.flags);
PIOS_Thread_Sleep(10);
for (uint32_t j = 0; j < MAXHOPS; j++) {
DEBUG_PRINTF(2, " hop channel: %d\r\n", openlrs_dev->bind_data.hopchannel[j]);
PIOS_Thread_Sleep(10);
}
}
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
if (openlrs_dev->bind_data.version == BINDING_VERSION) {
DEBUG_PRINTF(2, "data good\r\n");
rxb = 'B';
tx_packet(openlrs_dev, &rxb, 1); // ACK that we got bound
OPLinkSettingsData binding;
OPLinkSettingsGet(&binding);
binding.Version = openlrs_dev->bind_data.version;
binding.SerialBaudrate = openlrs_dev->bind_data.serial_baudrate;
binding.RFFrequency = openlrs_dev->bind_data.rf_frequency;
binding.CoordID = openlrs_dev->bind_data.rf_magic;
binding.RFPower = openlrs_dev->bind_data.rf_power;
binding.RFChannelSpacing = openlrs_dev->bind_data.rf_channel_spacing;
binding.ModemParams = openlrs_dev->bind_data.modem_params;
binding.Flags = openlrs_dev->bind_data.flags;
for (uint32_t j = 0; j < OPLINKSETTINGS_HOPCHANNEL_NUMELEM; j++) {
binding.HopChannel[j] = openlrs_dev->bind_data.hopchannel[j];
}
binding.BeaconFrequency = openlrs_dev->beacon_frequency;
binding.BeaconDelay = openlrs_dev->beacon_delay;
binding.BeaconPeriod = openlrs_dev->beacon_period;
OPLinkSettingsSet(&binding);
UAVObjSave(OPLinkSettingsHandle(), 0);
#if defined(PIOS_LED_LINK)
PIOS_LED_Toggle(PIOS_LED_LINK);
#endif /* PIOS_LED_LINK */
return 1;
}
} else {
rfm22_deassertCs(openlrs_dev);
rfm22_releaseBus(openlrs_dev);
}
openlrs_dev->rf_mode = Receive;
rx_reset(openlrs_dev);
}
}
return 0;
}
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
static void printVersion(uint16_t v)
{
char ver[8];
ver[0] = '0' + ((v >> 8) & 0x0f);
ver[1] = '.';
ver[2] = '0' + ((v >> 4) & 0x0f);
if (v & 0x0f) {
ver[3] = '.';
ver[4] = '0' + (v & 0x0f);
ver[5] = '\r';
ver[6] = '\n';
ver[7] = '\0';
} else {
ver[3] = '\r';
ver[4] = '\n';
ver[5] = '\0';
}
DEBUG_PRINTF(2, ver);
}
#endif /* if defined(PIOS_INCLUDE_DEBUG_CONSOLE) */
static void pios_openlrs_setup(struct pios_openlrs_dev *openlrs_dev, bool bind)
{
DEBUG_PRINTF(2, "OpenLRSng RX setup starting. Binding: %e\r\n", bind);
PIOS_Thread_Sleep(5);
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
printVersion(OPENLRSNG_VERSION);
#endif
// Get the OPLinkStatus UAVO
OPLinkStatusData oplink_status;
OPLinkStatusGet(&oplink_status);
if (bind) {
oplink_status.LinkState = OPLINKSTATUS_LINKSTATE_BINDING;
if (pios_openlrs_bind_receive(openlrs_dev, 0)) {
// TODO: save binding settings bindWriteEeprom();
DEBUG_PRINTF(2, "Saved bind data to EEPROM (not really yet -- TODO)\r\n");
}
}
oplink_status.LinkState = OPLINKSTATUS_LINKSTATE_BOUND;
DEBUG_PRINTF(2, "Entering normal mode\r\n");
init_rfm(openlrs_dev, 0); // Configure the RFM22B's registers for normal operation
openlrs_dev->rf_channel = 0;
rfmSetChannel(openlrs_dev, openlrs_dev->rf_channel);
// Count hopchannels as we need it later
openlrs_dev->hopcount = 0;
while ((openlrs_dev->hopcount < MAXHOPS) && (openlrs_dev->bind_data.hopchannel[openlrs_dev->hopcount] != 0)) {
openlrs_dev->hopcount++;
}
// ################### RX SYNC AT STARTUP #################
openlrs_dev->rf_mode = Receive;
to_rx_mode(openlrs_dev);
openlrs_dev->link_acquired = 0;
openlrs_dev->lastPacketTimeUs = PIOS_DELAY_GetuS();
// Update the OPLinkStatus UAVO
OPLinkStatusSet(&oplink_status);
DEBUG_PRINTF(2, "OpenLRSng RX setup complete\r\n");
}
static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
{
uint32_t timeUs, timeMs;
#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
// Update the watchdog timer
PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
#endif /* PIOS_WDG_RFM22B */
if (rfm22_read_claim(openlrs_dev, 0x0C) == 0) { // detect the locked module and reboot
DEBUG_PRINTF(2, "OLRS ERR: RX hang\r\n");
init_rfm(openlrs_dev, 0);
to_rx_mode(openlrs_dev);
}
// Get the OPLinkStatus UAVO
OPLinkStatusData oplink_status;
OPLinkStatusGet(&oplink_status);
// Update the RSSI
oplink_status.RSSI = openlrs_dev->rssi;
timeUs = PIOS_DELAY_GetuS();
timeMs = PIOS_Thread_Systime();
// DEBUG_PRINTF(2,"Time: %d\r\n", timeUs);
uint8_t *tx_buf = openlrs_dev->tx_buf; // convenient variable
if (openlrs_dev->rf_mode == Received) {
DEBUG_PRINTF(2, "Packet Received. Dt=%d\r\n", timeUs - openlrs_dev->lastPacketTimeUs);
// Read the packet from RFM22b
rfm22_claimBus(openlrs_dev);
rfm22_assertCs(openlrs_dev);
PIOS_SPI_TransferByte(openlrs_dev->spi_id, 0x7F);
uint32_t packet_size = getPacketSize(&openlrs_dev->bind_data);
PIOS_SPI_TransferBlock(openlrs_dev->spi_id, OUT_FF,
openlrs_dev->rx_buf, packet_size, NULL);
rfm22_deassertCs(openlrs_dev);
rfm22_releaseBus(openlrs_dev);
openlrs_dev->lastAFCCvalue = rfmGetAFCC(openlrs_dev);
#if defined(PIOS_LED_LINK)
PIOS_LED_Toggle(PIOS_LED_LINK);
#endif /* PIOS_LED_LINK */
openlrs_dev->lastPacketTimeUs = timeUs;
openlrs_dev->numberOfLostPackets = 0;
oplink_status.LinkQuality <<= 1;
oplink_status.LinkQuality |= 1;
if ((openlrs_dev->rx_buf[0] & 0x3e) == 0x00) {
// This flag indicates receiving PPM data
unpackChannels(openlrs_dev->bind_data.flags & 7, openlrs_dev->ppm, openlrs_dev->rx_buf + 1);
rescaleChannels(openlrs_dev->ppm);
// Call the PPM received callback if it's available.
if (openlrs_dev->openlrs_rcvr_id) {
#if defined(PIOS_INCLUDE_OPENLRS_RCVR)
PIOS_OpenLRS_Rcvr_UpdateChannels(openlrs_dev->openlrs_rcvr_id, openlrs_dev->ppm);
#endif
}
if (openlrs_dev->ppm_callback) {
openlrs_dev->ppm_callback(openlrs_dev->ppm);
}
} else {
// Not PPM data. Push into serial RX buffer.
if ((openlrs_dev->rx_buf[0] & 0x38) == 0x38) {
if ((openlrs_dev->rx_buf[0] ^ tx_buf[0]) & 0x80) {
// We got new data... (not retransmission)
tx_buf[0] ^= 0x80; // signal that we got it
bool rx_need_yield;
uint8_t data_len = openlrs_dev->rx_buf[0] & 7;
if (openlrs_dev->rx_in_cb && (data_len > 0)) {
(openlrs_dev->rx_in_cb)(openlrs_dev->rx_in_context, &openlrs_dev->rx_buf[1], data_len, NULL, &rx_need_yield);
}
}
}
}
// Flag to indicate ever got a link
openlrs_dev->link_acquired |= true;
oplink_status.LinkState = OPLINKSTATUS_LINKSTATE_CONNECTED;
openlrs_dev->beacon_armed = false; // when receiving packets make sure beacon cannot emit
// When telemetry is enabled we ack packets and send info about FC back
if (openlrs_dev->bind_data.flags & TELEMETRY_MASK) {
if ((tx_buf[0] ^ openlrs_dev->rx_buf[0]) & 0x40) {
// resend last message
} else {
tx_buf[0] &= 0xc0;
tx_buf[0] ^= 0x40; // swap sequence as we have new data
// Check for data on serial link
uint8_t bytes = 0;
// Append data from the com interface if applicable.
if (openlrs_dev->tx_out_cb) {
// Try to get some data to send
bool need_yield = false;
bytes = (openlrs_dev->tx_out_cb)(openlrs_dev->tx_out_context, &tx_buf[1], 8, NULL, &need_yield);
}
if (bytes > 0) {
tx_buf[0] |= (0x37 + bytes);
} else {
// tx_buf[0] lowest 6 bits left at 0
tx_buf[1] = openlrs_dev->rssi;
if (FlightBatteryStateHandle()) {
FlightBatteryStateData bat;
FlightBatteryStateGet(&bat);
// FrSky protocol normally uses 3.3V at 255 but
// divider from display can be set internally
tx_buf[2] = (uint8_t)bat.Voltage / 25.0f * 255;
tx_buf[3] = (uint8_t)bat.Current / 60.0f * 255;
} else {
tx_buf[2] = 0; // these bytes carry analog info. package
tx_buf[3] = 0; // battery here
}
tx_buf[4] = (openlrs_dev->lastAFCCvalue >> 8);
tx_buf[5] = openlrs_dev->lastAFCCvalue & 0xff;
tx_buf[6] = countSetBits(oplink_status.LinkQuality & 0x7fff);
}
}
// This will block until sent
tx_packet(openlrs_dev, tx_buf, 9);
}
// Once a packet has been processed, flip back into receiving mode
openlrs_dev->rf_mode = Receive;
rx_reset(openlrs_dev);
openlrs_dev->willhop = 1;
}
if (openlrs_dev->link_acquired) {
// For missing packets to be properly trigger a well timed channel hop, this method should be called fairly close (but not sooner)
// than 1ms after the packet was expected to trigger this path
if ((openlrs_dev->numberOfLostPackets < openlrs_dev->hopcount) && (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) > (getInterval(&openlrs_dev->bind_data) + packet_timeout_us))) {
DEBUG_PRINTF(2, "OLRS WARN: Lost packet: %d\r\n", openlrs_dev->numberOfLostPackets);
// we lost packet, hop to next channel
oplink_status.LinkQuality <<= 1;
openlrs_dev->willhop = 1;
if (openlrs_dev->numberOfLostPackets == 0) {
openlrs_dev->linkLossTimeMs = timeMs;
openlrs_dev->nextBeaconTimeMs = 0;
}
openlrs_dev->numberOfLostPackets++;
openlrs_dev->lastPacketTimeUs += getInterval(&openlrs_dev->bind_data);
openlrs_dev->willhop = 1;
} else if ((openlrs_dev->numberOfLostPackets >= openlrs_dev->hopcount) && (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) > (getInterval(&openlrs_dev->bind_data) * openlrs_dev->hopcount))) {
DEBUG_PRINTF(2, "ORLS WARN: Trying to resync\r\n");
// hop slowly to allow resync with TX
oplink_status.LinkQuality = 0;
openlrs_dev->willhop = 1;
openlrs_dev->lastPacketTimeUs = timeUs;
}
if (openlrs_dev->numberOfLostPackets) {
#if defined(PIOS_LED_LINK)
PIOS_LED_Off(PIOS_LED_LINK);
#endif /* PIOS_LED_LINK */
if (openlrs_dev->failsafeDelay &&
(oplink_status.LinkState == OPLINKSTATUS_LINKSTATE_CONNECTED) &&
((timeMs - openlrs_dev->linkLossTimeMs) > ((uint32_t)openlrs_dev->failsafeDelay))) {
DEBUG_PRINTF(2, "Failsafe activated: %d %d\r\n", timeMs, openlrs_dev->linkLossTimeMs);
oplink_status.LinkState = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
// failsafeApply();
openlrs_dev->nextBeaconTimeMs = (timeMs + 1000UL * openlrs_dev->beacon_period) | 1; // beacon activating...
}
if ((openlrs_dev->beacon_frequency) && (openlrs_dev->nextBeaconTimeMs) &&
((timeMs - openlrs_dev->nextBeaconTimeMs) < 0x80000000)) {
// Indicate that the beacon is now active so we can trigger extra ones below
openlrs_dev->beacon_armed = true;
DEBUG_PRINTF(2, "Beacon time: %d\r\n", openlrs_dev->nextBeaconTimeMs);
// Only beacon when disarmed
uint8_t armed;
FlightStatusArmedGet(&armed);
if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
beacon_send(openlrs_dev, false); // play cool tune
init_rfm(openlrs_dev, 0); // go back to normal RX
rx_reset(openlrs_dev);
openlrs_dev->nextBeaconTimeMs = (timeMs + 1000UL * openlrs_dev->beacon_period) | 1; // avoid 0 in time
}
}
}
} else {
// Waiting for first packet, hop slowly
if (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) > (getInterval(&openlrs_dev->bind_data) * openlrs_dev->hopcount)) {
// DEBUG_PRINTF(3,"Trying to get first packet\r\n");
openlrs_dev->lastPacketTimeUs = timeUs;
openlrs_dev->willhop = 1;
}
}
if (openlrs_dev->willhop == 1) {
openlrs_dev->rf_channel++;
if ((openlrs_dev->rf_channel == MAXHOPS) || (openlrs_dev->bind_data.hopchannel[openlrs_dev->rf_channel] == 0)) {
openlrs_dev->rf_channel = 0;
}
if ((openlrs_dev->beacon_frequency) && (openlrs_dev->nextBeaconTimeMs) && openlrs_dev->beacon_armed) {
// Listen for RSSI on beacon channel briefly for 'trigger'
uint8_t brssi = beaconGetRSSI(openlrs_dev);
if (brssi > ((openlrs_dev->beacon_rssi_avg >> 2) + 20)) {
openlrs_dev->nextBeaconTimeMs = timeMs + 1000L;
}
openlrs_dev->beacon_rssi_avg = (openlrs_dev->beacon_rssi_avg * 3 + brssi * 4) >> 2;
rfmSetCarrierFrequency(openlrs_dev, openlrs_dev->bind_data.rf_frequency);
}
rfmSetChannel(openlrs_dev, openlrs_dev->rf_channel);
rx_reset(openlrs_dev);
openlrs_dev->willhop = 0;
}
// Update UAVO
OPLinkStatusSet(&oplink_status);
}
uint8_t PIOS_OpenLRS_RSSI_Get(void)
{
// Get the OPLinkStatus UAVO
OPLinkStatusData oplink_status;
OPLinkStatusGet(&oplink_status);
if (oplink_status.LinkState != OPLINKSTATUS_LINKSTATE_CONNECTED) {
return 0;
} else {
OPLinkStatusData openlrs_status;
OPLinkStatusGet(&openlrs_status);
OPLinkSettingsData openlrs_data;
OPLinkSettingsGet(&openlrs_data);
uint16_t LQ = oplink_status.LinkQuality & 0x7fff;
// count number of 1s in LinkQuality
LQ = LQ - ((LQ >> 1) & 0x5555);
LQ = (LQ & 0x3333) + ((LQ >> 2) & 0x3333);
LQ = LQ + (LQ >> 4);
LQ &= 0x0F0F;
LQ = (LQ * 0x0101) >> 8;
switch (openlrs_data.RSSIType) {
case OPLINKSETTINGS_RSSITYPE_COMBINED:
if ((uint8_t)LQ == 15) {
return (uint8_t)((oplink_status.RSSI >> 1) + 128);
} else {
return LQ * 9;
}
case OPLINKSETTINGS_RSSITYPE_RSSI:
return openlrs_status.RSSI;
case OPLINKSETTINGS_RSSITYPE_LINKQUALITY:
return (uint8_t)(LQ << 4);
default:
return 0;
}
}
}
/*****************************************************************************
* PPM Code
*****************************************************************************/
/**
* Register a OpenLRS_Rcvr interface to inform of PPM packets
*
* @param[in] rfm22b_dev The RFM22B device ID.
* @param[in] rfm22b_rcvr_id The receiver device to inform of PPM packets
*/
void PIOS_OpenLRS_RegisterRcvr(uint32_t openlrs_id, uint32_t openlrs_rcvr_id)
{
struct pios_openlrs_dev *openlrs_dev =
(struct pios_openlrs_dev *)openlrs_id;
if (!pios_openlrs_validate(openlrs_dev)) {
return;
}
openlrs_dev->openlrs_rcvr_id = openlrs_rcvr_id;
}
/**
* Register a OpenLRS_Rcvr interface to inform of PPM packets using a generic callback.
*
* @param[in] openlrs_id The OpenLRS device ID.
* @param[in] callback The callback function.
*/
void PIOS_OpenLRS_RegisterPPMCallback(uint32_t openlrs_id, PIOS_OpenLRS_PPMReceivedCallback callback)
{
struct pios_openlrs_dev *openlrs_dev =
(struct pios_openlrs_dev *)openlrs_id;
if (!pios_openlrs_validate(openlrs_dev)) {
return;
}
openlrs_dev->ppm_callback = callback;
}
/*****************************************************************************
* Task and device setup
*****************************************************************************/
static void pios_openlrs_task(void *parameters);
// ! Global device handle, required for IRQ handler
static struct pios_openlrs_dev *g_openlrs_dev;
/**
* Initialise an OPENLRS device
*
* @param[out] openlrs_id A pointer to store the device ID in.
* @param[in] spi_id The SPI bus index.
* @param[in] slave_num The SPI bus slave number.
* @param[in] cfg The device configuration.
*/
int32_t PIOS_OpenLRS_Init(uint32_t *openlrs_id, uint32_t spi_id,
uint32_t slave_num,
const struct pios_openlrs_cfg *cfg)
{
PIOS_DEBUG_Assert(openlrs_id);
PIOS_DEBUG_Assert(cfg);
// Allocate the device structure.
struct pios_openlrs_dev *openlrs_dev = pios_openlrs_alloc();
if (!openlrs_dev) {
return -1;
}
*openlrs_id = (uint32_t)openlrs_dev;
g_openlrs_dev = openlrs_dev;
// Hardcode the band for now.
openlrs_dev->band = 430000000;
// Store the SPI handle
openlrs_dev->slave_num = slave_num;
openlrs_dev->spi_id = spi_id;
// Before initializing everything, make sure device found
uint8_t device_type = rfm22_read(openlrs_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK;
if (device_type != 0x08) {
return -1;
}
// Initialize the com callbacks.
openlrs_dev->rx_in_cb = NULL;
openlrs_dev->tx_out_cb = NULL;
// Initialize the "PPM" callback.
openlrs_dev->openlrs_rcvr_id = 0;
openlrs_dev->ppm_callback = 0;
OPLinkSettingsInitialize();
OPLinkStatusInitialize();
DEBUG_PRINTF(2, "OpenLRS UAVOs Initialized\r\n");
OPLinkSettingsData binding;
OPLinkSettingsGet(&binding);
if (binding.Version == BINDING_VERSION) {
openlrs_dev->bind_data.version = binding.Version;
openlrs_dev->bind_data.serial_baudrate = binding.SerialBaudrate;
openlrs_dev->bind_data.rf_frequency = binding.RFFrequency;
openlrs_dev->bind_data.rf_magic = binding.CoordID;
openlrs_dev->bind_data.rf_power = binding.RFPower;
openlrs_dev->bind_data.rf_channel_spacing = binding.RFChannelSpacing;
openlrs_dev->bind_data.modem_params = binding.ModemParams;
openlrs_dev->bind_data.flags = binding.Flags;
for (uint32_t i = 0; i < OPLINKSETTINGS_HOPCHANNEL_NUMELEM; i++) {
openlrs_dev->bind_data.hopchannel[i] = binding.HopChannel[i];
}
}
// Copy beacon settings over
openlrs_dev->beacon_frequency = binding.BeaconFrequency;
openlrs_dev->beacon_delay = binding.BeaconDelay;
openlrs_dev->beacon_period = binding.BeaconPeriod;
openlrs_dev->failsafeDelay = binding.FailsafeDelay;
// Bind the configuration to the device instance
openlrs_dev->cfg = *cfg;
// Initialize the external interrupt.
PIOS_EXTI_Init(cfg->exti_cfg);
// Register the watchdog timer for the radio driver task
#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
PIOS_WDG_RegisterFlag(PIOS_WDG_RFM22B);
#endif /* PIOS_WDG_RFM22B */
// Start the driver task. This task controls the radio state machine and removed all of the IO from the IRQ handler.
openlrs_dev->taskHandle = PIOS_Thread_Create(pios_openlrs_task, "PIOS_OpenLRS_Task", STACK_SIZE_BYTES, (void *)openlrs_dev, TASK_PRIORITY);
// TaskMonitorAdd(TASKINFO_RUNNING_MODEMRX, openlrs_dev->taskHandle);
return 0;
}
/**
* The task that controls the radio state machine.
*
* @param[in] paramters The task parameters.
*/
static void pios_openlrs_task(void *parameters)
{
struct pios_openlrs_dev *openlrs_dev = (struct pios_openlrs_dev *)parameters;
if (!pios_openlrs_validate(openlrs_dev)) {
return;
}
DEBUG_PRINTF(2, "Setup started\r\n");
if (openlrs_dev->bind_data.version == BINDING_VERSION) {
pios_openlrs_setup(openlrs_dev, false);
} else {
pios_openlrs_setup(openlrs_dev, true);
}
DEBUG_PRINTF(2, "Setup complete\r\n");
bool rssi_sampled = false;
while (1) {
#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
// Update the watchdog timer
PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
#endif /* PIOS_WDG_RFM22B */
/* This block of code determines the timing of when to call the loop method. It reaches a bit into
* the internal state of that method to get the optimal timings. This is to keep the loop method as
* similar as possible to the openLRSng implementation (for easier maintenance of compatibility)
* while minimizing overhead spinning in a while loop.
*
* There are three reasons to go into loop:
* 1. the ISR was triggered (packet was received)
* 2. a little before the expected packet (to sample the RSSI while receiving packet)
* 3. a little after expected packet (to channel hop when a packet was missing)
*/
uint32_t delay_ms = 0;
uint32_t time_since_packet_us = PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs);
if (!rssi_sampled) {
// If we had not sampled RSSI yet, schedule a bit early to try and catch while "packet is in the air"
uint32_t time_till_measure_rssi_us = (getInterval(&openlrs_dev->bind_data) - packet_advance_time_us) - time_since_packet_us;
delay_ms = (time_till_measure_rssi_us + 999) / 1000;
// DEBUG_PRINTF(3, "T1: %d\r\n", delay_ms);
} else {
// If we have sampled RSSI we want to schedule to hop when a packet has been missed
uint32_t time_till_timeout_us = (getInterval(&openlrs_dev->bind_data) + packet_timeout_us) - time_since_packet_us;
delay_ms = (time_till_timeout_us + 999) / 1000;
// DEBUG_PRINTF(3, "T2: %d %d\r\n", time_till_timeout_us,delay_ms, time_since_packet_us);
}
// Maximum delay based on packet time
const uint32_t max_delay = (getInterval(&openlrs_dev->bind_data) + packet_timeout_us) / 1000;
if (delay_ms > max_delay) {
delay_ms = max_delay;
}
if (PIOS_Semaphore_Take(openlrs_dev->sema_isr, delay_ms) == false) {
if (!rssi_sampled) {
// We timed out to sample RSSI
if (openlrs_dev->numberOfLostPackets < 2) {
openlrs_dev->lastRSSITimeUs = openlrs_dev->lastPacketTimeUs;
openlrs_dev->rssi = rfmGetRSSI(openlrs_dev); // Read the RSSI value
// DEBUG_PRINTF(3, "Sampled RSSI: %d %d\r\n", openlrs_dev->RSSI, delay);
}
} else {
// We timed out because packet was missed
DEBUG_PRINTF(3, "ISR Timeout. Missed packet: %d %d %d\r\n", delay, getInterval(&openlrs_dev->bind_data), time_since_packet_us);
pios_openlrs_rx_loop(openlrs_dev);
}
rssi_sampled = true;
} else {
// DEBUG_PRINTF(3, "ISR %d %d %d\r\n", delay, getInterval(&openlrs_dev->bind_data), time_since_packet_us);
// Process incoming data
pios_openlrs_rx_loop(openlrs_dev);
// When a packet has been received (processed below) indicate we need to sample a new RSSI
rssi_sampled = false;
}
// DEBUG_PRINTF(3, "Processing time %d\r\n", PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs));
}
}
bool PIOS_OpenLRS_EXT_Int(void)
{
struct pios_openlrs_dev *openlrs_dev = g_openlrs_dev;
if (!pios_openlrs_validate(openlrs_dev)) {
return false;
}
if (openlrs_dev->rf_mode == Transmit) {
openlrs_dev->rf_mode = Transmitted;
} else if (openlrs_dev->rf_mode == Receive) {
openlrs_dev->rf_mode = Received;
}
// Indicate to main task that an ISR occurred
bool woken = false;
PIOS_Semaphore_Give_FromISR(openlrs_dev->sema_isr, &woken);
return woken;
}
/**
* Allocate the device structure
*/
static struct pios_openlrs_dev *pios_openlrs_alloc(void)
{
struct pios_openlrs_dev *openlrs_dev;
openlrs_dev = (struct pios_openlrs_dev *)pios_malloc(sizeof(*openlrs_dev));
if (!openlrs_dev) {
return NULL;
}
openlrs_dev->spi_id = 0;
// Create the ISR signal
openlrs_dev->sema_isr = PIOS_Semaphore_Create();
if (!openlrs_dev->sema_isr) {
pios_free(openlrs_dev);
return NULL;
}
openlrs_dev->magic = PIOS_OPENLRS_DEV_MAGIC;
return openlrs_dev;
}
/**
* Validate that the device structure is valid.
*
* @param[in] openlrs_dev The OpenLRS device structure pointer.
*/
static bool pios_openlrs_validate(struct pios_openlrs_dev *openlrs_dev)
{
return openlrs_dev != NULL
&& openlrs_dev->magic == PIOS_OPENLRS_DEV_MAGIC;
}
/*****************************************************************************
* SPI Read/Write Functions
*****************************************************************************/
/**
* Assert the chip select line.
*
* @param[in] rfm22b_dev The RFM22B device.
*/
static void rfm22_assertCs(struct pios_openlrs_dev *openlrs_dev)
{
PIOS_DELAY_WaituS(1);
if (openlrs_dev->spi_id != 0) {
PIOS_SPI_RC_PinSet(openlrs_dev->spi_id,
openlrs_dev->slave_num, 0);
}
}
/**
* Deassert the chip select line.
*
* @param[in] rfm22b_dev The RFM22B device structure pointer.
*/
static void rfm22_deassertCs(struct pios_openlrs_dev *openlrs_dev)
{
if (openlrs_dev->spi_id != 0) {
PIOS_SPI_RC_PinSet(openlrs_dev->spi_id,
openlrs_dev->slave_num, 1);
}
}
/**
* Claim the SPI bus.
*
* @param[in] rfm22b_dev The RFM22B device structure pointer.
*/
static void rfm22_claimBus(struct pios_openlrs_dev *openlrs_dev)
{
if (openlrs_dev->spi_id != 0) {
PIOS_SPI_ClaimBus(openlrs_dev->spi_id);
}
}
/**
* Release the SPI bus.
*
* @param[in] rfm22b_dev The RFM22B device structure pointer.
*/
static void rfm22_releaseBus(struct pios_openlrs_dev *openlrs_dev)
{
if (openlrs_dev->spi_id != 0) {
PIOS_SPI_ReleaseBus(openlrs_dev->spi_id);
}
}
/**
* Claim the semaphore and write a byte to a register
*
* @param[in] rfm22b_dev The RFM22B device.
* @param[in] addr The address to write to
* @param[in] data The datat to write to that address
*/
static void rfm22_write_claim(struct pios_openlrs_dev *openlrs_dev,
uint8_t addr, uint8_t data)
{
rfm22_claimBus(openlrs_dev);
rfm22_assertCs(openlrs_dev);
uint8_t buf[2] = { addr | 0x80, data };
PIOS_SPI_TransferBlock(openlrs_dev->spi_id, buf, NULL, sizeof(buf),
NULL);
rfm22_deassertCs(openlrs_dev);
rfm22_releaseBus(openlrs_dev);
}
/**
* Claim the semaphore and write a byte to a register
*
* @param[in] rfm22b_dev The RFM22B device.
* @param[in] addr The address to write to
* @param[in] data The datat to write to that address
*/
static uint8_t rfm22_read_claim(struct pios_openlrs_dev *openlrs_dev,
uint8_t addr)
{
uint8_t out[2] = { addr &0x7F, 0xFF };
uint8_t in[2];
rfm22_claimBus(openlrs_dev);
rfm22_assertCs(openlrs_dev);
PIOS_SPI_TransferBlock(openlrs_dev->spi_id, out, in, sizeof(out),
NULL);
rfm22_deassertCs(openlrs_dev);
rfm22_releaseBus(openlrs_dev);
return in[1];
}
/**
* Write a byte to a register without claiming the semaphore
*
* @param[in] rfm22b_dev The RFM22B device.
* @param[in] addr The address to write to
* @param[in] data The datat to write to that address
*/
static void rfm22_write(struct pios_openlrs_dev *openlrs_dev, uint8_t addr,
uint8_t data)
{
rfm22_assertCs(openlrs_dev);
uint8_t buf[2] = { addr | 0x80, data };
PIOS_SPI_TransferBlock(openlrs_dev->spi_id, buf, NULL, sizeof(buf),
NULL);
rfm22_deassertCs(openlrs_dev);
}
/**
* Read a byte from an RFM22b register without claiming the bus
*
* @param[in] rfm22b_dev The RFM22B device structure pointer.
* @param[in] addr The address to read from
* @return Returns the result of the register read
*/
static uint8_t rfm22_read(struct pios_openlrs_dev *openlrs_dev, uint8_t addr)
{
uint8_t out[2] = { addr &0x7F, 0xFF };
uint8_t in[2];
rfm22_assertCs(openlrs_dev);
PIOS_SPI_TransferBlock(openlrs_dev->spi_id, out, in, sizeof(out),
NULL);
rfm22_deassertCs(openlrs_dev);
return in[1];
}
#endif /* PIOS_INCLUDE_OPENLRS */
/**
* @}
* @}
*/
Reference in New Issue View Git Blame Copy Permalink