zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ST24 lib: formatting

sbg
Lorenz Meier 10 years ago
parent
082a0c7aa5
commit
3fc064882f
2 changed files with 138 additions and 129 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 56
      src/lib/rc/st24.c
  2. 3
      src/lib/rc/st24.h

56
src/lib/rc/st24.c
Unescape View File

@ -52,12 +52,13 @@ enum ST24_DECODE_STATE {
ST24_DECODE_STATE_GOT_DATA ST24_DECODE_STATE_GOT_DATA
}; };
const char* decode_states[] = {"UNSYNCED", const char *decode_states[] = {"UNSYNCED",
"GOT_STX1", "GOT_STX1",
"GOT_STX2", "GOT_STX2",
"GOT_LEN", "GOT_LEN",
"GOT_TYPE", "GOT_TYPE",
"GOT_DATA"}; "GOT_DATA"
};
/* define range mapping here, -+100% -> 1000..2000 */ /* define range mapping here, -+100% -> 1000..2000 */
#define ST24_RANGE_MIN 0.0f #define ST24_RANGE_MIN 0.0f
@ -102,7 +103,8 @@ 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 *channel_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, uint16_t *channels,
uint16_t max_chan_count)
{ {
bool ret = false; bool ret = false;
@ -112,25 +114,31 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
if (byte == ST24_STX1) { if (byte == ST24_STX1) {
_decode_state = ST24_DECODE_STATE_GOT_STX1; _decode_state = ST24_DECODE_STATE_GOT_STX1;
} }
break; break;
case ST24_DECODE_STATE_GOT_STX1: case ST24_DECODE_STATE_GOT_STX1:
if (byte == ST24_STX2) { if (byte == ST24_STX2) {
_decode_state = ST24_DECODE_STATE_GOT_STX2; _decode_state = ST24_DECODE_STATE_GOT_STX2;
} else { } else {
_decode_state = ST24_DECODE_STATE_UNSYNCED; _decode_state = ST24_DECODE_STATE_UNSYNCED;
} }
break; break;
case ST24_DECODE_STATE_GOT_STX2: case ST24_DECODE_STATE_GOT_STX2:
/* ensure no data overflow failure or hack is possible */ /* ensure no data overflow failure or hack is possible */
if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
_rxpacket.length = byte; _rxpacket.length = byte;
_rxlen = 0; _rxlen = 0;
_decode_state = ST24_DECODE_STATE_GOT_LEN; _decode_state = ST24_DECODE_STATE_GOT_LEN;
} else { } else {
_decode_state = ST24_DECODE_STATE_UNSYNCED; _decode_state = ST24_DECODE_STATE_UNSYNCED;
} }
break; break;
case ST24_DECODE_STATE_GOT_LEN: case ST24_DECODE_STATE_GOT_LEN:
@ -142,16 +150,18 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
case ST24_DECODE_STATE_GOT_TYPE: case ST24_DECODE_STATE_GOT_TYPE:
_rxpacket.st24_data[_rxlen - 1] = byte; _rxpacket.st24_data[_rxlen - 1] = byte;
_rxlen++; _rxlen++;
if (_rxlen == (_rxpacket.length - 1)) { if (_rxlen == (_rxpacket.length - 1)) {
_decode_state = ST24_DECODE_STATE_GOT_DATA; _decode_state = ST24_DECODE_STATE_GOT_DATA;
} }
break; break;
case ST24_DECODE_STATE_GOT_DATA: case ST24_DECODE_STATE_GOT_DATA:
_rxpacket.crc8 = byte; _rxpacket.crc8 = byte;
_rxlen++; _rxlen++;
if (st24_common_crc8((uint8_t*)&(_rxpacket.length), _rxlen) == _rxpacket.crc8) { if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) {
ret = true; ret = true;
@ -159,9 +169,8 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
switch (_rxpacket.type) { switch (_rxpacket.type) {
case ST24_PACKET_TYPE_CHANNELDATA12: case ST24_PACKET_TYPE_CHANNELDATA12: {
{ ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data;
ChannelData12* d = (ChannelData12*)_rxpacket.st24_data;
*rssi = d->rssi; *rssi = d->rssi;
*rx_count = d->packet_count; *rx_count = d->packet_count;
@ -172,23 +181,22 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
for (unsigned i = 0; i < stride_count; i += 3) { for (unsigned i = 0; i < stride_count; i += 3) {
channels[chan_index] = ((uint16_t)d->channel[i] << 4); channels[chan_index] = ((uint16_t)d->channel[i] << 4);
channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++; chan_index++;
channels[chan_index] = ((uint16_t)d->channel[i+2]); channels[chan_index] = ((uint16_t)d->channel[i + 2]);
channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++; chan_index++;
} }
} }
break; break;
case ST24_PACKET_TYPE_CHANNELDATA24: case ST24_PACKET_TYPE_CHANNELDATA24: {
{ ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data;
ChannelData24* d = (ChannelData24*)&_rxpacket.st24_data;
*rssi = d->rssi; *rssi = d->rssi;
*rx_count = d->packet_count; *rx_count = d->packet_count;
@ -199,25 +207,24 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
for (unsigned i = 0; i < stride_count; i += 3) { for (unsigned i = 0; i < stride_count; i += 3) {
channels[chan_index] = ((uint16_t)d->channel[i] << 4); channels[chan_index] = ((uint16_t)d->channel[i] << 4);
channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++; chan_index++;
channels[chan_index] = ((uint16_t)d->channel[i+2]); channels[chan_index] = ((uint16_t)d->channel[i + 2]);
channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++; chan_index++;
} }
} }
break; break;
case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: {
{
ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket; // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data;
/* we silently ignore this data for now, as its not classic TX data */ /* we silently ignore this data for now, as it is unused */
ret = false; ret = false;
} }
break; break;
@ -231,6 +238,7 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
/* decoding failed */ /* decoding failed */
} }
_decode_state = ST24_DECODE_STATE_UNSYNCED; _decode_state = ST24_DECODE_STATE_UNSYNCED;
break; break;
} }

3
src/lib/rc/st24.h
Unescape View File

@ -157,6 +157,7 @@ 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* channel_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,
uint16_t *channels, uint16_t max_chan_count);
__END_DECLS __END_DECLS

Write Preview
Loading…
Cancel
Save