/**
 ******************************************************************************
 * @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
    }
}

#ifdef OPENLRS_SIMPLE_BEACON_TONE
// Basic tone
static void beacon_simpletone(struct pios_openlrs_dev *openlrs_dev, uint8_t tone, int16_t len __attribute__((unused)))
{
    DEBUG_PRINTF(2, "beacon_morse: %d\r\n", len);

#if defined(PIOS_LED_LINK)
    PIOS_LED_On(PIOS_LED_LINK);
#endif /* PIOS_LED_LINK */

    rfm22_claimBus(openlrs_dev);

    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);

    int16_t cycles = (len * 50) / tone;
    for (int16_t i = 0; i < cycles; i++) {
        GPIO_SetBits(gpio, pin_source);
        PIOS_Thread_Sleep(tone);
        GPIO_ResetBits(gpio, pin_source);
        PIOS_Thread_Sleep(tone);
    }
    GPIO_Init(gpio, (GPIO_InitTypeDef *)&openlrs_dev->cfg.spi_cfg->mosi.init);
    GPIO_PinAFConfig(gpio, pin_source, remap);
    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 */
}

#else /* OPENLRS_SIMPLE_BEACON_TONE */

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 */
}

#endif /* OPENLRS_SIMPLE_BEACON_TONE */

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) {
#ifdef OPENLRS_SIMPLE_BEACON_TONE
            beacon_simpletone(openlrs_dev, 2, 3);
#else
            beacon_tone(openlrs_dev, 440, 1);
#endif
        }
    } else {
#ifdef OPENLRS_SIMPLE_BEACON_TONE
        for (uint32_t tone = 1; tone < 4; tone++) {
            if (tone == 1) {
                rfm22_write(openlrs_dev, 0x6d, 0x05); // 5 set min power 25mW
            } else {
                rfm22_write(openlrs_dev, 0x6d, 0x00); // 0 set min power 1.3mW
            }
            // L - --- - -
            beacon_simpletone(openlrs_dev, tone, 1);
            PIOS_Thread_Sleep(100);
            beacon_simpletone(openlrs_dev, tone, 3);
            PIOS_Thread_Sleep(100);
            beacon_simpletone(openlrs_dev, tone, 1);
            PIOS_Thread_Sleep(100);
            beacon_simpletone(openlrs_dev, tone, 1);
            PIOS_Thread_Sleep(600);
            // P - --- --- -
            beacon_simpletone(openlrs_dev, tone, 1);
            PIOS_Thread_Sleep(100);
            beacon_simpletone(openlrs_dev, tone, 3);
            PIOS_Thread_Sleep(100);
            beacon_simpletone(openlrs_dev, tone, 3);
            PIOS_Thread_Sleep(100);
            beacon_simpletone(openlrs_dev, tone, 1);
            PIOS_Thread_Sleep(2000);
        }
#else /* ifdef OPENLRS_SIMPLE_BEACON_TONE */
        // 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);
#endif /* #ifdef OPENLRS_SIMPLE_BEACON_TONE */
    }
    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);

    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;
        openlrs_dev->link_quality <<= 1;
        openlrs_dev->link_quality  |= 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->ppm_callback) {
                openlrs_dev->ppm_callback(openlrs_dev->ppm_context, 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(openlrs_dev->link_quality & 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
            openlrs_dev->link_quality <<= 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
            openlrs_dev->link_quality     = 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_delay)) | 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;
    }

    if (oplink_status.LinkState > OPLINKSTATUS_LINKSTATE_DISCONNECTED) {
        // Convert raw Rssi to dBm
        oplink_status.RSSI = (int8_t)(openlrs_dev->rssi >> 1) - 122;

        // Count number of bits set in link_quality
        uint8_t linkquality_bits = countSetBits(openlrs_dev->link_quality & 0x7fff);
        // Translate link quality to 0 - 128 range
        oplink_status.LinkQuality = (linkquality_bits + 1) * 8;
    } else {
        oplink_status.LinkQuality = 0;
        oplink_status.RSSI = -127;
    }

    // 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 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, uint32_t context)
{
    struct pios_openlrs_dev *openlrs_dev =
        (struct pios_openlrs_dev *)openlrs_id;

    if (!pios_openlrs_validate(openlrs_dev)) {
        return;
    }

    /*
     * Order is important in these assignments since openlrs_task uses ppm_callback
     * field to determine if it's ok to dereference ppm_callback and ppm_context
     */
    openlrs_dev->ppm_context  = context;
    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->ppm_context  = 0;
    openlrs_dev->ppm_callback = 0;


    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_ms, 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_ms, 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 */

/**
 * @}
 * @}
 */