fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 16:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updates to ST24 decoding library

Browse Source
This commit is contained in:
Lorenz Meier
2014-10-06 19:20:39 +02:00
parent 966688d092
commit debff9e179
2 changed files with 64 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/rc/st24.c
+63 -23
View File
@@ -40,10 +40,11 @@
*/ */
#include <stdbool.h> #include <stdbool.h>
#include <stdio.h>
#include "st24.h" #include "st24.h"
enum ST24_DECODE_STATE { enum ST24_DECODE_STATE {
ST24_DECODE_STATE_UNSYNCED, ST24_DECODE_STATE_UNSYNCED = 0,
ST24_DECODE_STATE_GOT_STX1, ST24_DECODE_STATE_GOT_STX1,
ST24_DECODE_STATE_GOT_STX2, ST24_DECODE_STATE_GOT_STX2,
ST24_DECODE_STATE_GOT_LEN, ST24_DECODE_STATE_GOT_LEN,
@@ -51,6 +52,24 @@ enum ST24_DECODE_STATE {
ST24_DECODE_STATE_GOT_DATA 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 enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
static unsigned _rxlen; 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; 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; break;
case ST24_DECODE_STATE_GOT_STX2: case ST24_DECODE_STATE_GOT_STX2:
_rxpacket.length = byte; /* ensure no data overflow failure or hack is possible */
_rxlen = 0; if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
_decode_state = ST24_DECODE_STATE_GOT_LEN; _rxpacket.length = byte;
_rxlen = 0;
_decode_state = ST24_DECODE_STATE_GOT_LEN;
} else {
_decode_state = ST24_DECODE_STATE_UNSYNCED;
}
break; break;
case ST24_DECODE_STATE_GOT_LEN: 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; break;
case ST24_DECODE_STATE_GOT_TYPE: case ST24_DECODE_STATE_GOT_TYPE:
if (_rxlen < (_rxpacket.length - 1)) { _rxpacket.st24_data[_rxlen - 1] = byte;
_rxpacket.st24_data[_rxlen] = byte; _rxlen++;
_rxlen++; if (_rxlen == (_rxpacket.length - 1)) {
} else {
_decode_state = ST24_DECODE_STATE_GOT_DATA; _decode_state = ST24_DECODE_STATE_GOT_DATA;
} }
break; 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; _rxpacket.crc8 = byte;
_rxlen++; _rxlen++;
if (st24_common_crc8((uint8_t*)&(_rxpacket.length), sizeof(_rxpacket.length) + if (st24_common_crc8((uint8_t*)&(_rxpacket.length), _rxlen) == _rxpacket.crc8) {
sizeof(_rxpacket.st24_data) + sizeof(_rxpacket.type)) == _rxpacket.crc8) {
ret = true; 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; *rssi = d->rssi;
*rx_count = d->packet_count; *rx_count = d->packet_count;
*channel_count = 12;
for (unsigned i = 0; i < 1; i += 2) { for (unsigned i = 0; i < *channel_count; i += 2) {
channels[i] = ((uint16_t)d->channel[i]) << 8; channels[i] = ((uint16_t)d->channel[i]) << 4;
channels[i] |= (0xF & d->channel[i+1]); channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4);
channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; channels[i+1] = ((uint16_t)d->channel[i+2] << 4);
channels[i+1] |= d->channel[i+2]; 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; 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; *rssi = d->rssi;
*rx_count = d->packet_count; *rx_count = d->packet_count;
*channel_count = 24;
for (unsigned i = 0; i < 1; i += 2) { for (unsigned i = 0; i < *channel_count; i += 2) {
channels[i] = ((uint16_t)d->channel[i]) << 8; channels[i] = ((uint16_t)d->channel[i]) << 4;
channels[i] |= (0xF & d->channel[i+1]); channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4);
channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; channels[i+1] = ((uint16_t)d->channel[i+2] << 4);
channels[i+1] |= d->channel[i+2]; channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1]));
// XXX apply scaling
} }
} }
break; 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: default:
ret = false; ret = false;
break; break;
@@ -178,10 +217,11 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
} else { } else {
/* decoding failed */ /* decoding failed */
_decode_state = ST24_DECODE_STATE_UNSYNCED;
} }
_decode_state = ST24_DECODE_STATE_UNSYNCED;
break; break;
} }
return ret; return !ret;
} }
src/lib/rc/st24.h
+1 -1
View File
@@ -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 * @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 * @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 __END_DECLS