fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
src/lib/rc/st24.c
View File

@ -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 < 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]));
// 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;
}

2
src/lib/rc/st24.h
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
* @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