ardupilot/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp

/*
 * This file 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 file is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
/*
  FRSky FPort2 implementation, with thanks to FrSky for
  specification
 */

#include "AP_RCProtocol_FPort2.h"
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_Math/AP_Math.h>
#include <AP_Math/crc.h>
#include <stdio.h>

extern const AP_HAL::HAL& hal;

#define FRAME_LEN_16 0x18
#define FRAME_LEN_8  0x0D
#define FRAME_LEN_24 0x23
#define FRAME_LEN_DOWNLINK 0x08

#define FRAME_TYPE_CHANNEL 0xFF
#define FRAME_TYPE_FC 0x1B

#define MAX_CHANNELS 24

#define FLAGS_FAILSAFE_BIT	3
#define FLAGS_FRAMELOST_BIT	2

#define CHAN_SCALE_FACTOR1 1000U
#define CHAN_SCALE_FACTOR2 1600U
#define CHAN_SCALE_OFFSET 875U

struct PACKED FPort2_Frame {
    uint8_t len;
    uint8_t type;
    union {
        uint8_t data[36];
        struct PACKED {
            uint8_t prim;
            uint16_t appid;
            uint8_t data[4];
            uint8_t crc;
        } downlink;
    };
};

static_assert(sizeof(FPort2_Frame) == FPORT2_CONTROL_FRAME_SIZE, "FPort2_Frame incorrect size");

// constructor
AP_RCProtocol_FPort2::AP_RCProtocol_FPort2(AP_RCProtocol &_frontend, bool _inverted) :
    AP_RCProtocol_Backend(_frontend),
    inverted(_inverted)
{}

// decode a full FPort2 control frame
void AP_RCProtocol_FPort2::decode_control(const FPort2_Frame &frame)
{
    uint16_t values[MAX_CHANNELS];

    decode_11bit_channels(frame.data, chan_count, values, CHAN_SCALE_FACTOR1, CHAN_SCALE_FACTOR2, CHAN_SCALE_OFFSET);

    const uint8_t b_flags = frame.data[byte_input.control_len-5];
    const uint8_t b_rssi = frame.data[byte_input.control_len-4];

    bool failsafe = ((b_flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);

    // fport2 rssi 0-50, ardupilot rssi 0-255, scale factor 255/50=5.1
    const uint8_t scaled_rssi = MIN(b_rssi * 5.1f, 255);

    add_input(chan_count, values, failsafe, scaled_rssi);
}

/*
  decode a full FPort2 downlink frame
*/
void AP_RCProtocol_FPort2::decode_downlink(const FPort2_Frame &frame)
{
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
    /*
      if we are getting FPORT2 over a UART then we can ask the FrSky
      telem library for some passthrough data to send back, enabling
      telemetry on the receiver via the same uart pin as we use for
      incoming RC frames
     */

    AP_HAL::UARTDriver *uart = get_UART();
    if (!uart) {
        return;
    }

    // we're only interested in "flight controller" requests
    if (frame.type != FRAME_TYPE_FC) {
        return;
    }

    /*
      get SPort data from FRSky_Telem
      We save the data to a variable so in case we're late we'll
      send it in the next call, this prevents corruption of
      status text messages
     */
    if (!telem_data.available) {
        if (!AP_Frsky_Telem::get_telem_data(telem_data.frame, telem_data.appid, telem_data.data)) {
            return;
        }
        telem_data.available = true;
    }
    /*
      check that we haven't been too slow in responding to the new
      UART data. If we respond too late then we will corrupt the next
      incoming control frame
     */
    uint64_t tend = uart->receive_time_constraint_us(1);
    uint64_t now = AP_HAL::micros64();
    uint64_t tdelay = now - tend;
    if (tdelay > 2500) {
        // we've been too slow in responding
        return;
    }

    uint8_t buf[10];

    buf[0] = 0x08;
    buf[1] = frame.type;
    buf[2] = telem_data.frame;
    buf[3] = telem_data.appid & 0xFF;
    buf[4] = telem_data.appid >> 8;
    memcpy(&buf[5], &telem_data.data, 4);
    buf[9] = crc_sum8(&buf[1], 8);
    
    uart->write(buf, sizeof(buf));

    // get fresh telem_data in the next call
    telem_data.available = false;
#endif
}

/*
  process a FPort2 input pulse of the given width
 */
void AP_RCProtocol_FPort2::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
    if (have_UART()) {
        // if we can use a UART we would much prefer to, as it allows
        // us to send SPORT data out
        return;
    }
    uint32_t w0 = width_s0;
    uint32_t w1 = width_s1;
    if (inverted) {
        w0 = saved_width;
        w1 = width_s0;
        saved_width = width_s1;
    }
    uint8_t b;
    if (ss.process_pulse(w0, w1, b)) {
        _process_byte(ss.get_byte_timestamp_us(), b);
    }
}

// support byte input
void AP_RCProtocol_FPort2::_process_byte(uint32_t timestamp_us, uint8_t b)
{
    const bool have_frame_gap = (timestamp_us - byte_input.last_byte_us >= 2000U);

    byte_input.last_byte_us = timestamp_us;

    if (have_frame_gap) {
        // if we have a frame gap then this must be the start of a new
        // frame
        byte_input.ofs = 0;
    }

    if (byte_input.ofs == 0) {
        switch (b) {
        case FRAME_LEN_8:
            byte_input.control_len = 16;
            chan_count = 8;
            byte_input.is_downlink = false;
            break;
        case FRAME_LEN_16:
            byte_input.control_len = 27;
            chan_count = 16;
            byte_input.is_downlink = false;
            break;
        case FRAME_LEN_24:
            byte_input.control_len = 37;
            chan_count = 24;
            byte_input.is_downlink = false;
            break;
        case FRAME_LEN_DOWNLINK:
            byte_input.control_len = 10;
            byte_input.is_downlink = true;
            break;
        default:
            // definately not FPort2, missing header byte
            return;
        }
    }

    if (byte_input.ofs == 1) {
        if (!byte_input.is_downlink && b != FRAME_TYPE_CHANNEL) {
            // not channel data
            byte_input.ofs = 0;
            return;
        }
    }

    byte_input.buf[byte_input.ofs++] = b;

    const FPort2_Frame *frame = (const FPort2_Frame *)&byte_input.buf[0];

    if (byte_input.control_len > 2 && byte_input.ofs == byte_input.control_len) {
        if (!byte_input.is_downlink) {
            log_data(AP_RCProtocol::FPORT2, timestamp_us, byte_input.buf, byte_input.ofs);
            if (check_checksum()) {
                decode_control(*frame);
            }
        } else {
            // downlink packet
            if (check_checksum()) {
                decode_downlink(*frame);
            }
        }
        goto reset;
    }
    if (byte_input.ofs >= sizeof(byte_input.buf)) {
        goto reset;
    }
    return;

reset:
    byte_input.ofs = 0;
}

// check checksum byte
bool AP_RCProtocol_FPort2::check_checksum(void)
{
    return crc_sum8(&byte_input.buf[1], byte_input.control_len-1) == 0;
}

// support byte input
void AP_RCProtocol_FPort2::process_byte(uint8_t b, uint32_t baudrate)
{
    if (baudrate != 115200) {
        return;
    }
    _process_byte(AP_HAL::micros(), b);
}