Compile and link ST24 parser in IO firmware

2 files changed, 146 insertions, 20 deletions

diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c
index addbcd899..956a72bf3 100644
--- a/src/lib/rc/st24.c
+++ b/src/lib/rc/st24.c
@@ -39,6 +39,23 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include <stdbool.h>
+#include "st24.h"
+
+enum ST24_DECODE_STATE {
+	ST24_DECODE_STATE_UNSYNCED,
+	ST24_DECODE_STATE_GOT_STX1,
+	ST24_DECODE_STATE_GOT_STX2,
+	ST24_DECODE_STATE_GOT_LEN,
+	ST24_DECODE_STATE_GOT_TYPE,
+	ST24_DECODE_STATE_GOT_DATA
+};
+
+static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
+static unsigned _rxlen;
+
+static ReceiverFcPacket _rxpacket;
+
 uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
 {
 	uint8_t i, crc ;
@@ -66,6 +83,105 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
 }
 
 
-uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) {
+uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count)
+{
+
+	bool ret = false;
+
+	switch (_decode_state) {
+		case ST24_DECODE_STATE_UNSYNCED:
+			if (byte == ST24_STX1) {
+				_decode_state = ST24_DECODE_STATE_GOT_STX1;
+			}
+			break;
+
+		case ST24_DECODE_STATE_GOT_STX1:
+			if (byte == ST24_STX2) {
+				_decode_state = ST24_DECODE_STATE_GOT_STX2;
+			} else {
+				_decode_state = ST24_DECODE_STATE_UNSYNCED;
+			}
+			break;
+
+		case ST24_DECODE_STATE_GOT_STX2:
+			_rxpacket.length = byte;
+			_rxlen = 0;
+			_decode_state = ST24_DECODE_STATE_GOT_LEN;
+			break;
+
+		case ST24_DECODE_STATE_GOT_LEN:
+			_rxpacket.type = byte;
+			_rxlen++;
+			_decode_state = ST24_DECODE_STATE_GOT_TYPE;
+			break;
+
+		case ST24_DECODE_STATE_GOT_TYPE:
+			if (_rxlen < (_rxpacket.length - 1)) {
+				_rxpacket.st24_data[_rxlen] = byte;
+				_rxlen++;
+			} else {
+				_decode_state = ST24_DECODE_STATE_GOT_DATA;
+			}
+			break;
+
+		case ST24_DECODE_STATE_GOT_DATA:
+			_rxpacket.crc8 = byte;
+			_rxlen++;
+
+			if (st24_common_crc8((uint8_t*)&(_rxpacket.length), sizeof(_rxpacket.length) +
+				sizeof(_rxpacket.st24_data) + sizeof(_rxpacket.type)) == _rxpacket.crc8) {
 
-}
\ No newline at end of file
+				ret = true;
+
+				/* decode the actual packet */
+
+				switch (_rxpacket.type) {
+
+					case ST24_PACKET_TYPE_CHANNELDATA12:
+						{
+							ChannelData12* d = (ChannelData12*)&_rxpacket;
+
+							*rssi = d->rssi;
+							*rx_count = d->packet_count;
+
+							for (unsigned i = 0; i < 1; i += 2) {
+								channels[i] = ((uint16_t)d->channel[i]) << 8;
+								channels[i] |= (0xF & d->channel[i+1]);
+
+								channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4;
+								channels[i+1] |= d->channel[i+2];
+							}
+						}
+						break;
+
+					case ST24_PACKET_TYPE_CHANNELDATA24:
+						{
+							ChannelData24* d = (ChannelData12*)&_rxpacket;
+
+							*rssi = d->rssi;
+							*rx_count = d->packet_count;
+
+							for (unsigned i = 0; i < 1; i += 2) {
+								channels[i] = ((uint16_t)d->channel[i]) << 8;
+								channels[i] |= (0xF & d->channel[i+1]);
+
+								channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4;
+								channels[i+1] |= d->channel[i+2];
+							}
+						}
+						break;
+
+					default:
+						ret = false;
+						break;
+				}
+
+			} else {
+				/* decoding failed */
+				_decode_state = ST24_DECODE_STATE_UNSYNCED;
+			}
+			break;
+	}
+
+	return ret;
+}
diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h
index 3621e8506..24dbf8e51 100644
--- a/src/lib/rc/st24.h
+++ b/src/lib/rc/st24.h
@@ -39,22 +39,30 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#pragma once
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
 #define ST24_DATA_LEN_MAX	64
+#define ST24_STX1		0x55
+#define ST24_STX2		0x55
 
-enum {
+enum ST24_PACKET_TYPE {
 	ST24_PACKET_TYPE_CHANNELDATA12 = 0,
 	ST24_PACKET_TYPE_CHANNELDATA24,
 	ST24_PACKET_TYPE_TRANSMITTERGPSDATA
-} ST24_PACKET_TYPE;
+};
 
 #pragma pack(push, 1)
 typedef struct {
-	uint8_t	header1;				///< 0x55 for a valid packet
-	uint8_t	header2;				///< 0x55 for a valid packet
+	uint8_t	header1;			///< 0x55 for a valid packet
+	uint8_t	header2;			///< 0x55 for a valid packet
 	uint8_t	length;				///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8)
-	uint8_t	type;					///< from enum ST24_PACKET_TYPE
+	uint8_t	type;				///< from enum ST24_PACKET_TYPE
 	uint8_t	st24_data[ST24_DATA_LEN_MAX];
-	uint8_t	crc8;					///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
+	uint8_t	crc8;				///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
 } ReceiverFcPacket;
 
 /**
@@ -63,8 +71,8 @@ typedef struct {
  * This is incoming from the ST24
  */
 typedef struct {
-	uint16_t	t;						///< packet counter or clock
-	uint8_t	rssi					///< signal strength
+	uint16_t t;			///< packet counter or clock
+	uint8_t	rssi;			///< signal strength
 	uint8_t	packet_count;		///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
 	uint8_t	channel[18];		///< channel data, 12 channels (12 bit numbers)
 } ChannelData12;
@@ -74,8 +82,8 @@ typedef struct {
  *
  */
 typedef struct {
-	uint16_t	t;						///< packet counter or clock
-	uint8_t	rssi					///< signal strength
+	uint16_t t;			///< packet counter or clock
+	uint8_t	rssi;			///< signal strength
 	uint8_t	packet_count;		///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
 	uint8_t	channel[36];		///< channel data, 24 channels (12 bit numbers)
 } ChannelData24;
@@ -114,17 +122,17 @@ typedef struct {
  *
  */
 typedef struct {
-	uint16_t t;						///< packet counter or clock
-	int32_t	lat;					///< lattitude (degrees)	+/- 90 deg
-	int32_t	lon;					///< longitude (degrees)	+/- 180 deg
-	int32_t	alt;					///< 0.01m resolution, altitude (meters)
+	uint16_t t;			///< packet counter or clock
+	int32_t	lat;			///< lattitude (degrees)	+/- 90 deg
+	int32_t	lon;			///< longitude (degrees)	+/- 180 deg
+	int32_t	alt;			///< 0.01m resolution, altitude (meters)
 	int16_t	vx, vy, vz; 		///< velocity 0.01m res, +/-320.00 North-East- Down
-	uint8_t	nsat;					///<number of satellites
-	uint8_t	voltage; 			///< 25.4V	voltage = 5 + 255*0.1 = 30.5V, min=5V
-	uint8_t	current; 			///< 0.5A resolution
+	uint8_t	nsat;			///<number of satellites
+	uint8_t	voltage; 		///< 25.4V	voltage = 5 + 255*0.1 = 30.5V, min=5V
+	uint8_t	current; 		///< 0.5A resolution
 	int16_t	roll, pitch, yaw;	///< 0.01 degree resolution
 	uint8_t	motorStatus;		///< 1 bit per motor for status 1=good, 0= fail
-	uint8_t	imuStatus;			///< inertial measurement unit status
+	uint8_t	imuStatus;		///< inertial measurement unit status
 	uint8_t	pressCompassStatus;	///< baro / compass status
 } TelemetryData;
 
@@ -150,3 +158,5 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len);
  * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error
  */
 __EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count);
+
+__END_DECLS