diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-10-06 19:20:39 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-10-06 19:20:39 +0200 |
commit | debff9e1796fe035975307dfc9d3e4ed081e3ded (patch) | |
tree | 878ec31001d7ea3356e1483a8c556420a803e015 | |
parent | 966688d0921e397f7a19c8e9f3c9fd40b9561bba (diff) | |
download | px4-firmware-debff9e1796fe035975307dfc9d3e4ed081e3ded.tar.gz px4-firmware-debff9e1796fe035975307dfc9d3e4ed081e3ded.tar.bz2 px4-firmware-debff9e1796fe035975307dfc9d3e4ed081e3ded.zip |
Updates to ST24 decoding library
-rw-r--r-- | src/lib/rc/st24.c | 86 | ||||
-rw-r--r-- | src/lib/rc/st24.h | 2 |
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 |