Updates to ST24 decoding library

2 files changed, 64 insertions, 24 deletions

diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c
index 2ae576532..f43f2de7b 100644
--- a/src/lib/rc/st24.c
+++ b/src/lib/rc/st24.c
@@ -40,10 +40,11 @@
  */
 
 #include <stdbool.h>
+#include <stdio.h>
 #include "st24.h"
 
 enum ST24_DECODE_STATE {
-	ST24_DECODE_STATE_UNSYNCED,
+	ST24_DECODE_STATE_UNSYNCED = 0,
 	ST24_DECODE_STATE_GOT_STX1,
 	ST24_DECODE_STATE_GOT_STX2,
 	ST24_DECODE_STATE_GOT_LEN,
@@ -51,6 +52,24 @@ enum ST24_DECODE_STATE {
 	ST24_DECODE_STATE_GOT_DATA
 };
 
+const char* decode_states[] = {"UNSYNCED",
+	"GOT_STX1",
+	"GOT_STX2",
+	"GOT_LEN",
+	"GOT_TYPE",
+	"GOT_DATA"};
+
+/* define range mapping here, -+100% -> 1000..2000 */
+#define ST24_RANGE_MIN 0.0f
+#define ST24_RANGE_MAX 8000.0f
+
+#define ST24_TARGET_MIN 1000.0f
+#define ST24_TARGET_MAX 2000.0f
+
+/* pre-calculate the floating point stuff as far as possible at compile time */
+#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN))
+#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f))
+
 static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
 static unsigned _rxlen;
 
@@ -83,7 +102,7 @@ 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 *channel_count, int16_t *channels, uint16_t max_chan_count)
 {
 
 	bool ret = false;
@@ -104,9 +123,14 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
 			break;
 
 		case ST24_DECODE_STATE_GOT_STX2:
-			_rxpacket.length = byte;
-			_rxlen = 0;
-			_decode_state = ST24_DECODE_STATE_GOT_LEN;
+			/* ensure no data overflow failure or hack is possible */
+			if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
+				_rxpacket.length = byte;
+				_rxlen = 0;
+				_decode_state = ST24_DECODE_STATE_GOT_LEN;
+			} else {
+				_decode_state = ST24_DECODE_STATE_UNSYNCED;
+			}
 			break;
 
 		case ST24_DECODE_STATE_GOT_LEN:
@@ -116,10 +140,9 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
 			break;
 
 		case ST24_DECODE_STATE_GOT_TYPE:
-			if (_rxlen < (_rxpacket.length - 1)) {
-				_rxpacket.st24_data[_rxlen] = byte;
-				_rxlen++;
-			} else {
+			_rxpacket.st24_data[_rxlen - 1] = byte;
+			_rxlen++;
+			if (_rxlen == (_rxpacket.length - 1)) {
 				_decode_state = ST24_DECODE_STATE_GOT_DATA;
 			}
 			break;
@@ -128,8 +151,7 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
 			_rxpacket.crc8 = byte;
 			_rxlen++;
 
-			if (st24_common_crc8((uint8_t*)&(_rxpacket.length), sizeof(_rxpacket.length) +
-				sizeof(_rxpacket.st24_data) + sizeof(_rxpacket.type)) == _rxpacket.crc8) {
+			if (st24_common_crc8((uint8_t*)&(_rxpacket.length), _rxlen) == _rxpacket.crc8) {
 
 				ret = true;
 
@@ -143,13 +165,18 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
 
 							*rssi = d->rssi;
 							*rx_count = d->packet_count;
+							*channel_count = 12;
 
-							for (unsigned i = 0; i < 1; i += 2) {
-								channels[i] = ((uint16_t)d->channel[i]) << 8;
-								channels[i] |= (0xF & d->channel[i+1]);
+							for (unsigned i = 0; i < *channel_count; i += 2) {
+								channels[i] = ((uint16_t)d->channel[i]) << 4;
+								channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4);
 
-								channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4;
-								channels[i+1] |= d->channel[i+2];
+								channels[i+1] = ((uint16_t)d->channel[i+2] << 4);
+								channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1]));
+
+								/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+								// channels[i] = (uint16_t)(channels[i] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET;
+								// channels[i+1] = (uint16_t)(channels[i+1] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET;
 							}
 						}
 						break;
@@ -160,17 +187,29 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
 
 							*rssi = d->rssi;
 							*rx_count = d->packet_count;
+							*channel_count = 24;
+
+							for (unsigned i = 0; i < *channel_count; i += 2) {
+								channels[i] = ((uint16_t)d->channel[i]) << 4;
+								channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4);
 
-							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)d->channel[i+2] << 4);
+								channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1]));
 
-								channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4;
-								channels[i+1] |= d->channel[i+2];
+								// XXX apply scaling
 							}
 						}
 						break;
 
+					case ST24_PACKET_TYPE_TRANSMITTERGPSDATA:
+						{
+
+							ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket;
+							/* we silently ignore this data for now, as its not classic TX data */
+							ret = false;
+						}
+						break;
+
 					default:
 						ret = false;
 						break;
@@ -178,10 +217,11 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
 
 			} else {
 				/* decoding failed */
-				_decode_state = ST24_DECODE_STATE_UNSYNCED;
+				
 			}
+			_decode_state = ST24_DECODE_STATE_UNSYNCED;
 			break;
 	}
 
-	return ret;
+	return !ret;
 }
diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h
index 24dbf8e51..45a835f77 100644
--- a/src/lib/rc/st24.h
+++ b/src/lib/rc/st24.h
@@ -157,6 +157,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len);
  * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned
  * @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);
+__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, int16_t *channels, uint16_t max_chan_count);
 
 __END_DECLS