dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fix up ST24 lib

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
Show Stats 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))) {
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[]) {
{
//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));

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 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);
channels[i+1] = ((uint16_t)d->channel[i+2] << 4);
channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1]));
unsigned stride_count = (*channel_count * 3) / 2;
unsigned chan_index = 0;
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[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++;
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;
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;