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. 262
      src/lib/rc/st24.c
  2. 3
      src/lib/rc/st24.h

262
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,137 +103,144 @@ 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;
switch (_decode_state) { switch (_decode_state) {
case ST24_DECODE_STATE_UNSYNCED: case ST24_DECODE_STATE_UNSYNCED:
if (byte == ST24_STX1) { if (byte == ST24_STX1) {
_decode_state = ST24_DECODE_STATE_GOT_STX1; _decode_state = ST24_DECODE_STATE_GOT_STX1;
} }
break;
case ST24_DECODE_STATE_GOT_STX1: break;
if (byte == ST24_STX2) {
_decode_state = ST24_DECODE_STATE_GOT_STX2; case ST24_DECODE_STATE_GOT_STX1:
} else { if (byte == ST24_STX2) {
_decode_state = ST24_DECODE_STATE_UNSYNCED; _decode_state = ST24_DECODE_STATE_GOT_STX2;
}
break; } else {
_decode_state = ST24_DECODE_STATE_UNSYNCED;
case ST24_DECODE_STATE_GOT_STX2: }
/* ensure no data overflow failure or hack is possible */
if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { break;
_rxpacket.length = byte;
_rxlen = 0; case ST24_DECODE_STATE_GOT_STX2:
_decode_state = ST24_DECODE_STATE_GOT_LEN;
} else { /* ensure no data overflow failure or hack is possible */
_decode_state = ST24_DECODE_STATE_UNSYNCED; if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
} _rxpacket.length = byte;
break; _rxlen = 0;
_decode_state = ST24_DECODE_STATE_GOT_LEN;
case ST24_DECODE_STATE_GOT_LEN:
_rxpacket.type = byte; } else {
_rxlen++; _decode_state = ST24_DECODE_STATE_UNSYNCED;
_decode_state = ST24_DECODE_STATE_GOT_TYPE; }
break;
break;
case ST24_DECODE_STATE_GOT_TYPE:
_rxpacket.st24_data[_rxlen - 1] = byte; case ST24_DECODE_STATE_GOT_LEN:
_rxlen++; _rxpacket.type = byte;
if (_rxlen == (_rxpacket.length - 1)) { _rxlen++;
_decode_state = ST24_DECODE_STATE_GOT_DATA; _decode_state = ST24_DECODE_STATE_GOT_TYPE;
} break;
break;
case ST24_DECODE_STATE_GOT_TYPE:
case ST24_DECODE_STATE_GOT_DATA: _rxpacket.st24_data[_rxlen - 1] = byte;
_rxpacket.crc8 = byte; _rxlen++;
_rxlen++;
if (_rxlen == (_rxpacket.length - 1)) {
if (st24_common_crc8((uint8_t*)&(_rxpacket.length), _rxlen) == _rxpacket.crc8) { _decode_state = ST24_DECODE_STATE_GOT_DATA;
}
ret = true;
break;
/* decode the actual packet */
case ST24_DECODE_STATE_GOT_DATA:
switch (_rxpacket.type) { _rxpacket.crc8 = byte;
_rxlen++;
case ST24_PACKET_TYPE_CHANNELDATA12:
{ if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) {
ChannelData12* d = (ChannelData12*)_rxpacket.st24_data;
ret = true;
*rssi = d->rssi;
*rx_count = d->packet_count; /* decode the actual packet */
*channel_count = 12;
switch (_rxpacket.type) {
unsigned stride_count = (*channel_count * 3) / 2;
unsigned chan_index = 0; case ST24_PACKET_TYPE_CHANNELDATA12: {
ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data;
for (unsigned i = 0; i < stride_count; i += 3) {
channels[chan_index] = ((uint16_t)d->channel[i] << 4); *rssi = d->rssi;
channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); *rx_count = d->packet_count;
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ *channel_count = 12;
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET;
chan_index++; unsigned stride_count = (*channel_count * 3) / 2;
unsigned chan_index = 0;
channels[chan_index] = ((uint16_t)d->channel[i+2]);
channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); for (unsigned i = 0; i < stride_count; i += 3) {
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ channels[chan_index] = ((uint16_t)d->channel[i] << 4);
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
chan_index++; /* 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;
} chan_index++;
break;
channels[chan_index] = ((uint16_t)d->channel[i + 2]);
case ST24_PACKET_TYPE_CHANNELDATA24: channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
{ /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
ChannelData24* d = (ChannelData24*)&_rxpacket.st24_data; channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
*rssi = d->rssi; }
*rx_count = d->packet_count; }
*channel_count = 24; break;
unsigned stride_count = (*channel_count * 3) / 2; case ST24_PACKET_TYPE_CHANNELDATA24: {
unsigned chan_index = 0; ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data;
for (unsigned i = 0; i < stride_count; i += 3) { *rssi = d->rssi;
channels[chan_index] = ((uint16_t)d->channel[i] << 4); *rx_count = d->packet_count;
channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); *channel_count = 24;
/* 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; unsigned stride_count = (*channel_count * 3) / 2;
chan_index++; unsigned chan_index = 0;
channels[chan_index] = ((uint16_t)d->channel[i+2]); for (unsigned i = 0; i < stride_count; i += 3) {
channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); channels[chan_index] = ((uint16_t)d->channel[i] << 4);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
chan_index++; channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
} chan_index++;
}
break; channels[chan_index] = ((uint16_t)d->channel[i + 2]);
channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: /* 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;
chan_index++;
ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket; }
/* we silently ignore this data for now, as its not classic TX data */
ret = false;
}
break;
default:
ret = false;
break;
} }
break;
} else { case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: {
/* decoding failed */
// ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data;
/* we silently ignore this data for now, as it is unused */
ret = false;
}
break;
default:
ret = false;
break;
} }
_decode_state = ST24_DECODE_STATE_UNSYNCED;
break; } else {
/* decoding failed */
}
_decode_state = ST24_DECODE_STATE_UNSYNCED;
break;
} }
return !ret; return !ret;

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