diff options
Diffstat (limited to 'src/lib/rc/st24.c')
| -rw-r--r-- | src/lib/rc/st24.c | 120 |
1 files changed, 118 insertions, 2 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; +} |