PX4-Autopilot/src/lib/rc/crsf.cpp

/****************************************************************************
 *
 *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#if 0 // enable non-verbose debugging
#define CRSF_DEBUG PX4_WARN
#else
#define CRSF_DEBUG(...)
#endif

#if 0 // verbose debugging. Careful when enabling: it leads to too much output, causing dropped bytes
#define CRSF_VERBOSE PX4_WARN
#else
#define CRSF_VERBOSE(...)
#endif

#include <drivers/drv_hrt.h>
#include <termios.h>
#include <string.h>
#include <unistd.h>

#include "crsf.h"
#include "common_rc.h"

#define MIN(a,b) (((a)<(b))?(a):(b))
#define MAX(a,b) (((a)>(b))?(a):(b))


#define CRSF_BAUDRATE 420000

#define CRSF_SYNC_BYTE 0xC8

enum class crsf_frame_type_t : uint8_t {
	gps = 0x02,
	battery_sensor = 0x08,
	link_statistics = 0x14,
	rc_channels_packed = 0x16,
	attitude = 0x1E,
	flight_mode = 0x21,

	// Extended Header Frames, range: 0x28 to 0x96
	device_ping = 0x28,
	device_info = 0x29,
	parameter_settings_entry = 0x2B,
	parameter_read = 0x2C,
	parameter_write = 0x2D,
	command = 0x32
};

enum class crsf_payload_size_t : uint8_t {
	gps = 15,
	battery_sensor = 8,
	link_statistics = 10,
	rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
	attitude = 6,
};


enum class crsf_address_t : uint8_t {
	broadcast = 0x00,
	usb = 0x10,
	tbs_core_pnp_pro = 0x80,
	reserved1 = 0x8A,
	current_sensor = 0xC0,
	gps = 0xC2,
	tbs_blackbox = 0xC4,
	flight_controller = 0xC8,
	reserved2 = 0xCA,
	race_tag = 0xCC,
	radio_transmitter = 0xEA,
	crsf_receiver = 0xEC,
	crsf_transmitter = 0xEE
};

#pragma pack(push, 1)
struct crsf_payload_RC_channels_packed_t {
	// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes
	unsigned chan0 : 11;
	unsigned chan1 : 11;
	unsigned chan2 : 11;
	unsigned chan3 : 11;
	unsigned chan4 : 11;
	unsigned chan5 : 11;
	unsigned chan6 : 11;
	unsigned chan7 : 11;
	unsigned chan8 : 11;
	unsigned chan9 : 11;
	unsigned chan10 : 11;
	unsigned chan11 : 11;
	unsigned chan12 : 11;
	unsigned chan13 : 11;
	unsigned chan14 : 11;
	unsigned chan15 : 11;
};

#pragma pack(pop)

enum class crsf_parser_state_t : uint8_t {
	unsynced = 0,
	synced
};

static crsf_frame_t	 &crsf_frame = rc_decode_buf.crsf_frame;
static unsigned current_frame_position = 0;
static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced;

/**
 * parse the current crsf_frame buffer
 */
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels);

static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a);
static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len);
uint8_t crsf_frame_CRC(const crsf_frame_t &frame);


int
crsf_config(int uart_fd)
{
	struct termios t;

	/* no parity, one stop bit */
	tcgetattr(uart_fd, &t);
	cfsetspeed(&t, CRSF_BAUDRATE);
	t.c_cflag &= ~(CSTOPB | PARENB);
	return tcsetattr(uart_fd, TCSANOW, &t);
}

/**
 * Convert from RC to PWM value
 * @param chan_value channel value in [172, 1811]
 * @return PWM channel value in [1000, 2000]
 */
static uint16_t convert_channel_value(unsigned chan_value);


bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values,
		uint16_t *num_values, uint16_t max_channels)
{
	bool ret = false;
	uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;

	while (len > 0) {

		// fill in the crsf_buffer, as much as we can
		const unsigned current_len = MIN(len, sizeof(crsf_frame_t) - current_frame_position);
		memcpy(crsf_frame_ptr + current_frame_position, frame, current_len);
		current_frame_position += current_len;

		// protection to guarantee parsing progress
		if (current_len == 0) {
			CRSF_DEBUG("========== parser bug: no progress (%i) ===========", len);

			for (unsigned i = 0; i < current_frame_position; ++i) {
				CRSF_DEBUG("crsf_frame_ptr[%i]: 0x%x", i, (int)crsf_frame_ptr[i]);
			}

			// reset the parser
			current_frame_position = 0;
			parser_state = crsf_parser_state_t::unsynced;
			return false;
		}

		len -= current_len;
		frame += current_len;

		if (crsf_parse_buffer(values, num_values, max_channels)) {
			ret = true;
		}
	}


	return ret;
}

static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
{
	crc ^= a;

	for (int i = 0; i < 8; ++i) {
		if (crc & 0x80) {
			crc = (crc << 1) ^ 0xD5;

		} else {
			crc = crc << 1;
		}
	}

	return crc;
}

static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
{
	uint8_t crc = 0;

	for (int i = 0; i < len; ++i) {
		crc = crc8_dvb_s2(crc, buf[i]);
	}

	return crc;
}

uint8_t crsf_frame_CRC(const crsf_frame_t &frame)
{
	// CRC includes type and payload
	uint8_t crc = crc8_dvb_s2(0, frame.type);

	for (int i = 0; i < frame.header.length - 2; ++i) {
		crc = crc8_dvb_s2(crc, frame.payload[i]);
	}

	return crc;
}

static uint16_t convert_channel_value(unsigned chan_value)
{
	/*
	 *       RC     PWM
	 * min  172 ->  988us
	 * mid  992 -> 1500us
	 * max 1811 -> 2012us
	 */
	static constexpr float scale = (2012.f - 988.f) / (1811.f - 172.f);
	static constexpr float offset = 988.f - 172.f * scale;
	return (scale * chan_value) + offset;
}

static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
{
	uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;

	if (parser_state == crsf_parser_state_t::unsynced) {
		// there is no sync byte, try to find an RC packet by searching for a matching frame length and type
		for (unsigned i = 1; i < current_frame_position - 1; ++i) {
			if (crsf_frame_ptr[i] == (uint8_t)crsf_payload_size_t::rc_channels + 2 &&
			    crsf_frame_ptr[i + 1] == (uint8_t)crsf_frame_type_t::rc_channels_packed) {
				parser_state = crsf_parser_state_t::synced;
				unsigned frame_offset = i - 1;
				CRSF_VERBOSE("RC channels found at offset %i", frame_offset);

				// move the rest of the buffer to the beginning
				if (frame_offset != 0) {
					memmove(crsf_frame_ptr, crsf_frame_ptr + frame_offset, current_frame_position - frame_offset);
					current_frame_position -= frame_offset;
				}

				break;
			}
		}
	}

	if (parser_state != crsf_parser_state_t::synced) {
		if (current_frame_position >= sizeof(crsf_frame_t)) {
			// discard most of the data, but keep the last 3 bytes (otherwise we could miss the frame start)
			current_frame_position = 3;

			for (unsigned i = 0; i < current_frame_position; ++i) {
				crsf_frame_ptr[i] = crsf_frame_ptr[sizeof(crsf_frame_t) - current_frame_position + i];
			}

			CRSF_VERBOSE("Discarding buffer");
		}

		return false;
	}


	if (current_frame_position < 3) {
		// wait until we have the header & type
		return false;
	}

	// Now we have at least the header and the type

	const unsigned current_frame_length = crsf_frame.header.length + sizeof(crsf_frame_header_t);

	if (current_frame_length > sizeof(crsf_frame_t) || current_frame_length < 4) {
		// frame too long or bogus -> discard everything and go into unsynced state
		current_frame_position = 0;
		parser_state = crsf_parser_state_t::unsynced;
		CRSF_DEBUG("Frame too long/bogus (%i, type=%i) -> unsync", current_frame_length, crsf_frame.type);
		return false;
	}

	if (current_frame_position < current_frame_length) {
		// we don't have the full frame yet -> wait for more data
		CRSF_VERBOSE("waiting for more data (%i < %i)", current_frame_position, current_frame_length);
		return false;
	}

	bool ret = false;

	// Now we have the full frame

	if (crsf_frame.type == (uint8_t)crsf_frame_type_t::rc_channels_packed &&
	    crsf_frame.header.length == (uint8_t)crsf_payload_size_t::rc_channels + 2) {
		const uint8_t crc = crsf_frame.payload[crsf_frame.header.length - 2];

		if (crc == crsf_frame_CRC(crsf_frame)) {
			const crsf_payload_RC_channels_packed_t *const rc_channels =
				(crsf_payload_RC_channels_packed_t *)&crsf_frame.payload;
			*num_values = MIN(max_channels, 16);

			if (max_channels > 0) { values[0] = convert_channel_value(rc_channels->chan0); }

			if (max_channels > 1) { values[1] = convert_channel_value(rc_channels->chan1); }

			if (max_channels > 2) { values[2] = convert_channel_value(rc_channels->chan2); }

			if (max_channels > 3) { values[3] = convert_channel_value(rc_channels->chan3); }

			if (max_channels > 4) { values[4] = convert_channel_value(rc_channels->chan4); }

			if (max_channels > 5) { values[5] = convert_channel_value(rc_channels->chan5); }

			if (max_channels > 6) { values[6] = convert_channel_value(rc_channels->chan6); }

			if (max_channels > 7) { values[7] = convert_channel_value(rc_channels->chan7); }

			if (max_channels > 8) { values[8] = convert_channel_value(rc_channels->chan8); }

			if (max_channels > 9) { values[9] = convert_channel_value(rc_channels->chan9); }

			if (max_channels > 10) { values[10] = convert_channel_value(rc_channels->chan10); }

			if (max_channels > 11) { values[11] = convert_channel_value(rc_channels->chan11); }

			if (max_channels > 12) { values[12] = convert_channel_value(rc_channels->chan12); }

			if (max_channels > 13) { values[13] = convert_channel_value(rc_channels->chan13); }

			if (max_channels > 14) { values[14] = convert_channel_value(rc_channels->chan14); }

			if (max_channels > 15) { values[15] = convert_channel_value(rc_channels->chan15); }

			CRSF_VERBOSE("Got Channels");

			ret = true;

		} else {
			CRSF_DEBUG("CRC check failed");
		}

	} else {
		CRSF_DEBUG("Got Non-RC frame (len=%i, type=%i)", current_frame_length, crsf_frame.type);
		// We could check the CRC here and reset the parser into unsynced state if it fails.
		// But in practise it's robust even without that.
	}

	// Either reset or move the rest of the buffer
	if (current_frame_position > current_frame_length) {
		CRSF_VERBOSE("Moving buffer (%i > %i)", current_frame_position, current_frame_length);
		memmove(crsf_frame_ptr, crsf_frame_ptr + current_frame_length, current_frame_position - current_frame_length);
		current_frame_position -= current_frame_length;

	} else {
		current_frame_position = 0;
	}

	return ret;
}

/**
 * write an uint8_t value to a buffer at a given offset and increment the offset
 */
static inline void write_uint8_t(uint8_t *buf, int &offset, uint8_t value)
{
	buf[offset++] = value;
}
/**
 * write an uint16_t value to a buffer at a given offset and increment the offset
 */
static inline void write_uint16_t(uint8_t *buf, int &offset, uint16_t value)
{
	// Big endian
	buf[offset] = value >> 8;
	buf[offset + 1] = value & 0xff;
	offset += 2;
}
/**
 * write an uint24_t value to a buffer at a given offset and increment the offset
 */
static inline void write_uint24_t(uint8_t *buf, int &offset, int value)
{
	// Big endian
	buf[offset] = value >> 16;
	buf[offset + 1] = (value >> 8) & 0xff;
	buf[offset + 2] = value & 0xff;
	offset += 3;
}

/**
 * write an int32_t value to a buffer at a given offset and increment the offset
 */
static inline void write_int32_t(uint8_t *buf, int &offset, int32_t value)
{
	// Big endian
	buf[offset] = value >> 24;
	buf[offset + 1] = (value >> 16) & 0xff;
	buf[offset + 2] = (value >> 8) & 0xff;
	buf[offset + 3] = value & 0xff;
	offset += 4;
}

static inline void write_frame_header(uint8_t *buf, int &offset, crsf_frame_type_t type, uint8_t payload_size)
{
	write_uint8_t(buf, offset, CRSF_SYNC_BYTE); // this got changed from the address to the sync byte
	write_uint8_t(buf, offset, payload_size + 2);
	write_uint8_t(buf, offset, (uint8_t)type);
}
static inline void write_frame_crc(uint8_t *buf, int &offset, int buf_size)
{
	// CRC does not include the address and length
	write_uint8_t(buf, offset, crc8_dvb_s2_buf(buf + 2, buf_size - 3));

	// check correctness of buffer size (only needed during development)
	//if (buf_size != offset) { PX4_ERR("frame size mismatch (%i != %i)", buf_size, offset); }
}

bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, int fuel, uint8_t remaining)
{
	uint8_t buf[(uint8_t)crsf_payload_size_t::battery_sensor + 4];
	int offset = 0;
	write_frame_header(buf, offset, crsf_frame_type_t::battery_sensor, (uint8_t)crsf_payload_size_t::battery_sensor);
	write_uint16_t(buf, offset, voltage);
	write_uint16_t(buf, offset, current);
	write_uint24_t(buf, offset, fuel);
	write_uint8_t(buf, offset, remaining);
	write_frame_crc(buf, offset, sizeof(buf));
	return write(uart_fd, buf, offset) == offset;
}

bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed,
			     uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites)
{
	uint8_t buf[(uint8_t)crsf_payload_size_t::gps + 4];
	int offset = 0;
	write_frame_header(buf, offset, crsf_frame_type_t::gps, (uint8_t)crsf_payload_size_t::gps);
	write_int32_t(buf, offset, latitude);
	write_int32_t(buf, offset, longitude);
	write_uint16_t(buf, offset, groundspeed);
	write_uint16_t(buf, offset, gps_heading);
	write_uint16_t(buf, offset, altitude);
	write_uint8_t(buf, offset, num_satellites);
	write_frame_crc(buf, offset, sizeof(buf));
	return write(uart_fd, buf, offset) == offset;
}

bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw)
{
	uint8_t buf[(uint8_t)crsf_payload_size_t::attitude + 4];
	int offset = 0;
	write_frame_header(buf, offset, crsf_frame_type_t::attitude, (uint8_t)crsf_payload_size_t::attitude);
	write_uint16_t(buf, offset, pitch);
	write_uint16_t(buf, offset, roll);
	write_uint16_t(buf, offset, yaw);
	write_frame_crc(buf, offset, sizeof(buf));
	return write(uart_fd, buf, offset) == offset;
}

bool crsf_send_telemetry_flight_mode(int uart_fd, const char *flight_mode)
{
	const int max_length = 16;
	int length = strlen(flight_mode) + 1;

	if (length > max_length) {
		length = max_length;
	}

	uint8_t buf[max_length + 4];
	int offset = 0;
	write_frame_header(buf, offset, crsf_frame_type_t::flight_mode, length);
	memcpy(buf + offset, flight_mode, length);
	offset += length;
	buf[offset - 1] = 0; // ensure null-terminated string
	write_frame_crc(buf, offset, length + 4);
	return write(uart_fd, buf, offset) == offset;
}