zr-v4/libraries/AP_HAL_Linux/RCOutput_qflight.cpp

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
   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, see <http://www.gnu.org/licenses/>.
 */
/*
  this is a driver for RC output in the QFLIGHT board. Output goes via
  a UART with a CRC. See libraries/RC_Channel/examples/RC_UART for an
  example of the other end of this protocol
 */
#include <AP_HAL/AP_HAL.h>

#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT

#include "RCOutput_qflight.h"
#include <AP_HAL_Linux/qflight/qflight_util.h>
#include <AP_HAL_Linux/qflight/qflight_dsp.h>
#include <GCS_MAVLink/include/mavlink/v1.0/checksum.h>
#include <stdio.h>

extern const AP_HAL::HAL& hal;

using namespace Linux;

void RCOutput_QFLIGHT::init()
{
    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&RCOutput_QFLIGHT::timer_update, void));
}

void RCOutput_QFLIGHT::set_device_path(const char *_device)
{
    device = _device;
}


void RCOutput_QFLIGHT::set_freq(uint32_t chmask, uint16_t freq_hz)
{
    // no support for changing frequency yet
}

uint16_t RCOutput_QFLIGHT::get_freq(uint8_t ch)
{
    // return fixed fake value - no control of frequency over the UART
    return 490;
}

void RCOutput_QFLIGHT::enable_ch(uint8_t ch)
{
    if (ch >= channel_count) {
        return;
    }
    enable_mask |= 1U<<ch;
}

void RCOutput_QFLIGHT::disable_ch(uint8_t ch)
{
    if (ch >= channel_count) {
        return;
    }
    enable_mask &= ~1U<<ch;
}

void RCOutput_QFLIGHT::write(uint8_t ch, uint16_t period_us)
{
    if (ch >= channel_count) {
        return;
    }
    period[ch] = period_us;
    need_write = true;
}

uint16_t RCOutput_QFLIGHT::read(uint8_t ch)
{
    if (ch >= channel_count) {
        return 0;
    }
    return period[ch];
}

void RCOutput_QFLIGHT::read(uint16_t *period_us, uint8_t len)
{
    for (int i = 0; i < len; i++) {
        period_us[i] = read(i);
    }
}

void RCOutput_QFLIGHT::timer_update(void)
{
    /*
      we defer the open to the time to ensure all RPC calls are made
      from the same thread
     */
    if (fd == -1 && device != nullptr) {
        int ret = qflight_UART_open(device, &fd);
        printf("Opened ESC UART %s ret=%d fd=%d\n",
               device, ret, (int)fd);
        if (fd != -1) {
            qflight_UART_set_baudrate(fd, baudrate);
        }
    }
    if (!need_write || fd == -1) {
        return;
    }
    /*
      this implements the PWM over UART prototocol. 
     */
    struct PACKED {
        uint8_t magic = 0xF7;
        uint16_t period[channel_count];
        uint16_t crc;
    } frame;
    memcpy(frame.period, period, sizeof(period));
    frame.crc = crc_calculate((uint8_t*)frame.period, channel_count*2);
    int32_t nwritten;
    qflight_UART_write(fd, (uint8_t *)&frame, sizeof(frame), &nwritten);
    need_write = false;

    check_rc_in();
}

/*
  we accept RC input from the UART and treat it as RC overrides. This
  is an lazy way to allow an RCOutput driver to do RCInput. See the
  RC_UART example for the other end of this protocol
 */
void RCOutput_QFLIGHT::check_rc_in(void)
{
    const uint8_t magic = 0xf6;
    while (nrcin_bytes != sizeof(rcu.bytes)) {
        int32_t nread;
        if (qflight_UART_read(fd, rcu.bytes, sizeof(rcu.bytes)-nrcin_bytes, &nread) != 0 || nread <= 0) {
            return;
        }
        nrcin_bytes += nread;
        if (rcu.rcin.magic != magic) {
            for (uint8_t i=1; i<nrcin_bytes; i++) {
                if (rcu.bytes[i] == magic) {
                    memmove(&rcu.bytes[0], &rcu.bytes[i], nrcin_bytes-i);
                    nrcin_bytes = nrcin_bytes - i;
                    return;
                }
            }
            nrcin_bytes = 0;
            return;
        }
    }
    if (nrcin_bytes == sizeof(rcu.bytes)) {
        if (rcu.rcin.magic == 0xf6 &&
            crc_calculate((uint8_t*)rcu.rcin.rcin, sizeof(rcu.rcin.rcin)) == rcu.rcin.crc) {
            bool have_data = false;
            for (uint8_t i=0; i<8; i++) {
                if (rcu.rcin.rcin[i] != 0) {
                    have_data = true;
                    break;
                }
            }
            if (have_data) {
                hal.rcin->set_overrides((int16_t*)rcu.rcin.rcin, 8);
            }
        }
        nrcin_bytes = 0;
    }
}

#endif // CONFIG_HAL_BOARD_SUBTYPE