yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix up ST24 lib

Browse Source
This commit is contained in:
Lorenz Meier
2014-10-07 07:14:28 +02:00
parent debff9e179
commit 9c89499696
2 changed files with 36 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -36,7 +36,7 @@ int main(int argc, char *argv[]) {
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
if (((f - last_time) * 1000 * 1000) > 3000) { 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); uint8_t b = static_cast<uint8_t>(x);
@@ -56,8 +56,13 @@ int main(int argc, char *argv[]) {
{ {
//warnx("decoded: %u channels", (unsigned)channel_count); //warnx("decoded: %u channels", (unsigned)channel_count);
for (unsigned i = 0; i < channel_count; i++) { 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)); //warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast<unsigned>(rssi), static_cast<unsigned>(rx_count));

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

@@ -61,7 +61,7 @@ const char* decode_states[] = {"UNSYNCED",
/* 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
#define ST24_RANGE_MAX 8000.0f #define ST24_RANGE_MAX 4096.0f
#define ST24_TARGET_MIN 1000.0f #define ST24_TARGET_MIN 1000.0f
#define ST24_TARGET_MAX 2000.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: case ST24_PACKET_TYPE_CHANNELDATA12:
{ {
ChannelData12* d = (ChannelData12*)&_rxpacket; ChannelData12* d = (ChannelData12*)_rxpacket.st24_data;
*rssi = d->rssi; *rssi = d->rssi;
*rx_count = d->packet_count; *rx_count = d->packet_count;
*channel_count = 12; *channel_count = 12;
for (unsigned i = 0; i < *channel_count; i += 2) { unsigned stride_count = (*channel_count * 3) / 2;
channels[i] = ((uint16_t)d->channel[i]) << 4; unsigned chan_index = 0;
channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4);
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 */ /* 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[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET;
// channels[i+1] = (uint16_t)(channels[i+1] * 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[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET;
chan_index++;
} }
} }
break; break;
case ST24_PACKET_TYPE_CHANNELDATA24: case ST24_PACKET_TYPE_CHANNELDATA24:
{ {
ChannelData24* d = (ChannelData24*)&_rxpacket; ChannelData24* d = (ChannelData24*)&_rxpacket.st24_data;
*rssi = d->rssi; *rssi = d->rssi;
*rx_count = d->packet_count; *rx_count = d->packet_count;
*channel_count = 24; *channel_count = 24;
for (unsigned i = 0; i < *channel_count; i += 2) { unsigned stride_count = (*channel_count * 3) / 2;
channels[i] = ((uint16_t)d->channel[i]) << 4; unsigned chan_index = 0;
channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4);
channels[i+1] = ((uint16_t)d->channel[i+2] << 4); for (unsigned i = 0; i < stride_count; i += 3) {
channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); 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; break;