From bbae5047d19d5cc8b7727b20ea80db7f7b5b2c3f Mon Sep 17 00:00:00 2001 From: Joel Wetzell Date: Mon, 10 Aug 2020 22:55:20 -0500 Subject: [PATCH] Fix ADSB heading math and add heading to ADSB log --- firmware/application/apps/ui_adsb_rx.cpp | 3 +++ firmware/common/adsb.cpp | 11 +++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/firmware/application/apps/ui_adsb_rx.cpp b/firmware/application/apps/ui_adsb_rx.cpp index 52c557561..bd158620d 100644 --- a/firmware/application/apps/ui_adsb_rx.cpp +++ b/firmware/application/apps/ui_adsb_rx.cpp @@ -244,6 +244,9 @@ void ADSBRxView::on_frame(const ADSBFrameMessage * message) { } } else if(msg_type == 19 && msg_sub >= 1 && msg_sub <= 4){ entry.set_frame_velo(frame); + logentry += "Type:" + to_string_dec_uint(msg_sub) + + " Hdg:" + to_string_dec_uint(entry.velo.heading) + + " Spd: "+ to_string_dec_int(entry.velo.speed); if (send_updates) details_view->update(entry); } diff --git a/firmware/common/adsb.cpp b/firmware/common/adsb.cpp index 6a4412fec..3f8f4c231 100644 --- a/firmware/common/adsb.cpp +++ b/firmware/common/adsb.cpp @@ -332,16 +332,19 @@ adsb_vel decode_frame_velo(ADSBFrame& frame){ } if(velo_type == 1 || velo_type == 2){ //Ground Speed - int32_t velo_ew = (((frame_data[5] & 0x03) << 8) | frame_data[6]) - 1; - int32_t velo_ns = ((frame_data[7] & 0x7f) << 3) | ((frame_data[8]) >> 5) - 1; + int32_t raw_ew = ((frame_data[5] & 0x03) << 8) | frame_data[6]; + int32_t velo_ew = raw_ew - 1; //velocities are all offset by one (this is part of the spec) + + int32_t raw_ns = ((frame_data[7] & 0x7f) << 3) | (frame_data[8] >> 5); + int32_t velo_ns = raw_ns - 1; if (velo_type == 2){ // supersonic indicator so multiply by 4 velo_ew = velo_ew << 2; velo_ns = velo_ns << 2; } - if((frame_data[5]&4) >> 2) velo_ew *= -1; //check ew direction sign - if((frame_data[7]&0x80) >> 7) velo_ns *= -1; //check ns direction sign + if(frame_data[5]&0x04) velo_ew *= -1; //check ew direction sign + if(frame_data[7]&0x80) velo_ns *= -1; //check ns direction sign velo.speed = sqrt(velo_ns*velo_ns + velo_ew*velo_ew);