LibrePilot/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c

/**
 ******************************************************************************
 * @addtogroup OpenPilotModules OpenPilot Modules
 * @{
 * @addtogroup UAVOFrSKYSPortBridge UAVO to FrSKY S.PORT Bridge Module
 * @{
 *
 * @file       uavofrskysportbridge.c
 * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017-2019
 *             Tau Labs, http://taulabs.org, Copyright (C) 2014
 * @brief      Bridges selected UAVObjects to FrSKY Smart Port bus
 *
 * Since there is no public documentation of SmartPort protocol available,
 * this was put together by studying OpenTx source code, reading
 * tidbits of informations on the Internet and experimenting.
 * @see https://github.com/opentx/opentx/tree/next/radio/src/telemetry
 * @see https://code.google.com/p/telemetry-convert/wiki/FrSkySPortProtocol
 * @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 "frsky_packing.h"

#include "pios_board_io.h"

#include "barosensor.h"
#include "flightbatterysettings.h"
#include "flightbatterystate.h"
#include "gpspositionsensor.h"
#include "frskysporttelemetrysettings.h"
#include "hwsettings.h"
#include "taskinfo.h"


#define FRSKY_POLL_REQUEST          0x7e
#define FRSKY_MINIMUM_POLL_INTERVAL 10000

enum frsky_state {
    FRSKY_STATE_WAIT_POLL_REQUEST,
    FRSKY_STATE_WAIT_SENSOR_ID,
    FRSKY_STATE_WAIT_TX_DONE,
};

// Set of objects sent by this module
static const struct frsky_value_item frsky_value_items[] = {
    { FRSKY_GPS_COURSE_ID,  100,  frsky_encode_gps_course, 0 }, // attitude yaw estimate
    { FRSKY_ALT_ID,         100,  frsky_encode_altitude,   0 }, // altitude estimate
    { FRSKY_VARIO_ID,       100,  frsky_encode_vario,      0 }, // vertical speed
    { FRSKY_CURR_ID,        300,  frsky_encode_current,    0 }, // battery current
    { FRSKY_CELLS_ID,       850,  frsky_encode_cells,      0 }, // battery cells 1-2
    { FRSKY_CELLS_ID,       850,  frsky_encode_cells,      1 }, // battery cells 3-4
    { FRSKY_CELLS_ID,       850,  frsky_encode_cells,      2 }, // battery cells 5-6
    { FRSKY_CELLS_ID,       850,  frsky_encode_cells,      3 }, // battery cells 7-8
    { FRSKY_CELLS_ID,       850,  frsky_encode_cells,      4 }, // battery cells 9-10
    { FRSKY_CELLS_ID,       850,  frsky_encode_cells,      5 }, // battery cells 11-12
    { FRSKY_T1_ID,          2000, frsky_encode_t1,         0 }, // baro temperature
    { FRSKY_T2_ID,          1500, frsky_encode_t2,         0 }, // encodes GPS status!
    { FRSKY_FUEL_ID,        600,  frsky_encode_fuel,       0 }, // consumed battery energy
    { FRSKY_ACCX_ID,        100,  frsky_encode_acc,        0 }, // accX
    { FRSKY_ACCY_ID,        100,  frsky_encode_acc,        1 }, // accY
    { FRSKY_ACCZ_ID,        100,  frsky_encode_acc,        2 }, // accZ
    { FRSKY_GPS_LON_LAT_ID, 100,  frsky_encode_gps_coord,  0 }, // lattitude
    { FRSKY_GPS_LON_LAT_ID, 100,  frsky_encode_gps_coord,  1 }, // longitude
    { FRSKY_GPS_ALT_ID,     750,  frsky_encode_gps_alt,    0 }, // gps altitude
    { FRSKY_GPS_SPEED_ID,   200,  frsky_encode_gps_speed,  0 }, // gps speed
    { FRSKY_GPS_TIME_ID,    8000, frsky_encode_gps_time,   0 }, // gps date
    { FRSKY_GPS_TIME_ID,    2000, frsky_encode_gps_time,   1 }, // gps time
    { FRSKY_RPM_ID,         1500, frsky_encode_rpm,        0 }, // encodes flight status!
    { FRSKY_AIR_SPEED_ID,   100,  frsky_encode_airspeed,   0 }, // airspeed
};

struct frsky_sport_telemetry {
    enum frsky_state state;
    int32_t   scheduled_item;
    uint32_t  last_poll_time;
    uintptr_t com;
    bool ignore_echo;
    uint8_t   ignore_rx_chars;
    struct frsky_settings frsky_settings;
    uint16_t  schedule_nr;
    uint32_t  item_last_triggered[NELEMENTS(frsky_value_items)];
    uint16_t  item_schedule_nr[NELEMENTS(frsky_value_items)];
};

static const uint8_t frsky_sensor_ids[] = { 0x1b };

#define FRSKY_SPORT_BAUDRATE 57600

#if defined(PIOS_FRSKY_SPORT_TELEMETRY_STACK_SIZE)
#define STACK_SIZE_BYTES     PIOS_FRSKY_SPORT_TELEMETRY_STACK_SIZE
#else
#define STACK_SIZE_BYTES     672
#endif
#define TASK_PRIORITY        (tskIDLE_PRIORITY)

static struct frsky_sport_telemetry *frsky = 0;
static int32_t uavoFrSKYSPortBridgeInitialize(void);
static void uavoFrSKYSPortBridgeTask(void *parameters);

/**
 * Scan for value item with the longest expired time and schedule it to send in next poll turn
 *
 */

static void frsky_schedule_next_item(void)
{
    frsky->scheduled_item = -1;

    for (uint32_t i = 0; i < NELEMENTS(frsky_value_items); i++) {
        if (frsky_value_items[i].encode_value(&frsky->frsky_settings, 0, true, frsky_value_items[i].fn_arg)) {
            if (frsky->item_schedule_nr[i] == frsky->schedule_nr) {
                continue;
            }
            if (PIOS_DELAY_GetuSSince(frsky->item_last_triggered[i]) > (frsky_value_items[i].period_ms * 1000)) {
                frsky->scheduled_item = i;
                frsky->item_schedule_nr[i] = frsky->schedule_nr;
                break;
            }
        }
    }

    if (frsky->scheduled_item < 0) {
        frsky->schedule_nr++;
    }
}


/**
 * Send value item previously scheduled by frsky_schedule_next_itme()
 * @returns true when item value was sended
 */
static bool frsky_send_scheduled_item(void)
{
    int32_t item = frsky->scheduled_item;

    if ((item >= 0) && (item < (int32_t)NELEMENTS(frsky_value_items))) {
        frsky->item_last_triggered[item] = PIOS_DELAY_GetuS();
        uint32_t value = 0;
        if (frsky_value_items[item].encode_value(&frsky->frsky_settings, &value, false,
                                                 frsky_value_items[item].fn_arg)) {
            frsky->ignore_rx_chars += frsky_send_frame(frsky->com, (uint16_t)(frsky_value_items[item].id), value, false);

            return true;
        }
    }

    return false;
}

/**
 * Process incoming bytes from FrSky S.PORT bus
 * @param[in] b received byte
 */
static void frsky_receive_byte(uint8_t b)
{
    uint32_t i = 0;

    switch (frsky->state) {
    case FRSKY_STATE_WAIT_TX_DONE:
        // because RX and TX are connected, we need to ignore bytes
        // transmited by us
        if (--frsky->ignore_rx_chars == 0) {
            frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST;
        }
        break;

    case FRSKY_STATE_WAIT_POLL_REQUEST:
        if (b == FRSKY_POLL_REQUEST) {
            // X8R is polling us every 13ms
            if (PIOS_DELAY_GetuSSince(frsky->last_poll_time) > FRSKY_MINIMUM_POLL_INTERVAL) {
                frsky->last_poll_time = PIOS_DELAY_GetuS();
                frsky->state = FRSKY_STATE_WAIT_SENSOR_ID;
            }
        }
        break;

    case FRSKY_STATE_WAIT_SENSOR_ID:

        frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST;

        for (i = 0; i < sizeof(frsky_sensor_ids); i++) {
            if (frsky_sensor_ids[i] == b) {
                // get GPSPositionData once here to be more efficient, since
                // GPS position data are very often used by encode() handlers
                if (GPSPositionSensorHandle() != NULL) {
                    GPSPositionSensorGet(&frsky->frsky_settings.gps_position);
                }
                // send item previously scheduled
                frsky_send_scheduled_item();

                if (frsky->ignore_echo && frsky->ignore_rx_chars) {
                    frsky->state = FRSKY_STATE_WAIT_TX_DONE;
                }

                // schedule item for next poll turn
                frsky_schedule_next_item();
                break;
            }
        }
        break;
    }
}

/**
 * Module start routine automatically called after initialization routine
 * @return 0 when was successful
 */
static int32_t uavoFrSKYSPortBridgeStart(void)
{
    if (!frsky) {
        return -1;
    }

    if (FlightBatterySettingsHandle() != NULL
        && FlightBatteryStateHandle() != NULL) {
// TODO: maybe get this setting from somewhere else?
// uint8_t currentPin;
// FlightBatterySettingsCurrentPinGet(&currentPin);
// if (currentPin != FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE)
        frsky->frsky_settings.use_current_sensor = true;
        FlightBatterySettingsGet(&frsky->frsky_settings.battery_settings);
        frsky->frsky_settings.batt_cell_count    = frsky->frsky_settings.battery_settings.NbCells;
    }
// This is just to check if barometer is enabled.
// if (BaroSensorHandle() != NULL
// && PIOS_SENSORS_GetQueue(PIOS_SENSOR_BARO) != NULL)
    frsky->frsky_settings.use_baro_sensor = true;


    xTaskHandle task;
    xTaskCreate(uavoFrSKYSPortBridgeTask, "FrSky SPort Telemetry", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &task);
    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FRSKYSPORTTELEMETRY, task);

    return 0;
}

static void FrSKYSPortTelemetrySettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
    FrSKYSPortTelemetrySettingsData settings;

    FrSKYSPortTelemetrySettingsGet(&settings);
}


/**
 * Module initialization routine
 * @return 0 when initialization was successful
 */
static int32_t uavoFrSKYSPortBridgeInitialize(void)
{
    FrSKYSPortTelemetrySettingsInitialize();

    if (PIOS_COM_FRSKY_SPORT) {
        frsky = pios_malloc(sizeof(struct frsky_sport_telemetry));

        if (frsky != NULL) {
            memset(frsky, 0x00, sizeof(struct frsky_sport_telemetry));

            frsky->frsky_settings.use_current_sensor = false;
            frsky->frsky_settings.batt_cell_count    = 0;
            frsky->frsky_settings.use_baro_sensor    = false;
            frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST;
            frsky->last_poll_time = PIOS_DELAY_GetuS();
            frsky->scheduled_item = -1;
            frsky->com   = PIOS_COM_FRSKY_SPORT;
            frsky->ignore_echo = true; // This has to be true when RX & TX hw serial lines are connected. Otherwise false. (enforced below by setting half duplex. this makes internal connection between rx and tx
            // connect TX pin of flight controller to UNINVERTED SPort
            // (F4 based controllers do not have TX inversion capability.
            // Use external inverter or solder to uninverted receiver pin)
            // TODO: Add code/PIOS driver to enable inversion for F3 based convertes
            // TODO: implement FPORT driver
            frsky->schedule_nr = 1;

            uint8_t i;
            for (i = 0; i < NELEMENTS(frsky_value_items); i++) {
                frsky->item_last_triggered[i] = PIOS_DELAY_GetuS();
            }

            // Set Port options:
            // BAUD rate
            PIOS_COM_ChangeBaud(frsky->com, FRSKY_SPORT_BAUDRATE);

            HwSettingsSPortModeOptions options;
            HwSettingsSPortModeGet(&options);
            bool halfduplex;
            enum PIOS_USART_Inverted inverted;
            switch (options) {
            case HWSETTINGS_SPORTMODE_HALFDUPLEXNONINVERTED:
                halfduplex = true;
                inverted   = PIOS_USART_Inverted_None;
                break;
            case HWSETTINGS_SPORTMODE_HALFDUPLEXINVERTED:
                halfduplex = true;
                inverted   = PIOS_USART_Inverted_RxTx;
                break;
            case HWSETTINGS_SPORTMODE_FULLDUPLEXNONINVERTED:
                halfduplex = false;
                inverted   = PIOS_USART_Inverted_None;
                break;
            case HWSETTINGS_SPORTMODE_FULLDUPLEXINVERTED:
                halfduplex = false;
                inverted   = PIOS_USART_Inverted_RxTx;
                break;
            }

            // Port Inversion (Not available on STM32F4, will have no effect)
            PIOS_COM_Ioctl(frsky->com, PIOS_IOCTL_USART_SET_INVERTED, &inverted);

            // HalfDplex mode (Not available on STM32F0, will have no effect)
            PIOS_COM_Ioctl(frsky->com, PIOS_IOCTL_USART_SET_HALFDUPLEX, &halfduplex);

            FrSKYSPortTelemetrySettingsConnectCallback(FrSKYSPortTelemetrySettingsUpdatedCb);

            FrSKYSPortTelemetrySettingsUpdatedCb(0);

            return 0;
        }
    }

    return -1;
}
MODULE_INITCALL(uavoFrSKYSPortBridgeInitialize, uavoFrSKYSPortBridgeStart);

/**
 * Main task routine
 * @param[in] parameters parameter given by PIOS_Thread_Create()
 */
static void uavoFrSKYSPortBridgeTask(__attribute__((unused)) void *parameters)
{
    while (1) {
        uint8_t b = 0;
        uint16_t count = PIOS_COM_ReceiveBuffer(frsky->com, &b, 1, 0xffffffff);
        if (count) {
            frsky_receive_byte(b);
        }
    }
}

/**
 * @}
 * @}
 */