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

Fix up ST24 lib

sbg
Lorenz Meier 11 years ago
parent
debff9e179
commit
9c89499696
2 changed files with 35 additions and 18 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 9
      Tools/tests-host/st24_test.cpp
  2. 44
      src/lib/rc/st24.c

9
Tools/tests-host/st24_test.cpp
Unescape View File

@ -36,7 +36,7 @@ int main(int argc, char *argv[]) { @@ -36,7 +36,7 @@ int main(int argc, char *argv[]) {
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
if (((f - last_time) * 1000 * 1000) > 3000) {
warnx("FRAME RESET\n\n");
//warnx("FRAME RESET\n\n");
}
uint8_t b = static_cast<uint8_t>(x);
@ -56,8 +56,13 @@ int main(int argc, char *argv[]) { @@ -56,8 +56,13 @@ int main(int argc, char *argv[]) {
{
//warnx("decoded: %u channels", (unsigned)channel_count);
for (unsigned i = 0; i < channel_count; i++) {
warnx("channel %u: %d", i, static_cast<int>(channels[i]));
int16_t val = channels[i];
// if (i == 6)
warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
}
// unsigned chan = 1;
// warnx("channel %u: %d", chan, static_cast<int>(channels[chan]));
}
//warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast<unsigned>(rssi), static_cast<unsigned>(rx_count));

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

@ -61,7 +61,7 @@ const char* decode_states[] = {"UNSYNCED", @@ -61,7 +61,7 @@ const char* decode_states[] = {"UNSYNCED",
/* define range mapping here, -+100% -> 1000..2000 */
#define ST24_RANGE_MIN 0.0f
#define ST24_RANGE_MAX 8000.0f
#define ST24_RANGE_MAX 4096.0f
#define ST24_TARGET_MIN 1000.0f
#define ST24_TARGET_MAX 2000.0f
@ -161,42 +161,54 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch @@ -161,42 +161,54 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch
case ST24_PACKET_TYPE_CHANNELDATA12:
{
ChannelData12* d = (ChannelData12*)&_rxpacket;
ChannelData12* d = (ChannelData12*)_rxpacket.st24_data;
*rssi = d->rssi;
*rx_count = d->packet_count;
*channel_count = 12;
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);
unsigned stride_count = (*channel_count * 3) / 2;
unsigned chan_index = 0;
channels[i+1] = ((uint16_t)d->channel[i+2] << 4);
channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1]));
for (unsigned i = 0; i < stride_count; i += 3) {
channels[chan_index] = ((uint16_t)d->channel[i] << 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 */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET;
chan_index++;
channels[chan_index] = ((uint16_t)d->channel[i+2]);
channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8);
/* 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;
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET;
chan_index++;
}
}
break;
case ST24_PACKET_TYPE_CHANNELDATA24:
{
ChannelData24* d = (ChannelData24*)&_rxpacket;
ChannelData24* d = (ChannelData24*)&_rxpacket.st24_data;
*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);
unsigned stride_count = (*channel_count * 3) / 2;
unsigned chan_index = 0;
channels[i+1] = ((uint16_t)d->channel[i+2] << 4);
channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1]));
for (unsigned i = 0; i < stride_count; i += 3) {
channels[chan_index] = ((uint16_t)d->channel[i] << 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 */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET;
chan_index++;
// XXX apply scaling
channels[chan_index] = ((uint16_t)d->channel[i+2]);
channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8);
/* 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;

Write Preview
Loading…
Cancel
Save