From 9c89499696baa15f709d125e46866e9f11ad5432 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 07:14:28 +0200 Subject: [PATCH] Fix up ST24 lib --- Tools/tests-host/st24_test.cpp | 9 +++++-- src/lib/rc/st24.c | 44 +++++++++++++++++++++------------- 2 files changed, 35 insertions(+), 18 deletions(-) diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp index e1134bba63..f143542bd0 100644 --- a/Tools/tests-host/st24_test.cpp +++ b/Tools/tests-host/st24_test.cpp @@ -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(x); @@ -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(channels[i])); + + int16_t val = channels[i]; + // if (i == 6) + warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } + // unsigned chan = 1; + // warnx("channel %u: %d", chan, static_cast(channels[chan])); } //warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast(rssi), static_cast(rx_count)); diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index f43f2de7ba..5a13649b5a 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -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 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;