From 2902365b8024ab924d4b3545186c77839ad89acf Mon Sep 17 00:00:00 2001 From: Brumi-2021 Date: Sun, 1 Jan 2023 23:23:06 +0100 Subject: [PATCH] Solving correct Bearing Angle in ADSB TX app --- firmware/common/adsb.cpp | 20 ++++++++++++++------ firmware/common/sine_table.hpp | 2 +- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/firmware/common/adsb.cpp b/firmware/common/adsb.cpp index d1d11707..e8481f6f 100644 --- a/firmware/common/adsb.cpp +++ b/firmware/common/adsb.cpp @@ -311,18 +311,26 @@ void encode_frame_velo(ADSBFrame& frame, const uint32_t ICAO_address, const uint uint32_t velo_ew_abs, velo_ns_abs, v_rate_coded_abs; // To get NS and EW speeds from speed and bearing, a polar to cartesian conversion is enough - velo_ew = static_cast(sin_f32(DEG_TO_RAD(angle) + (pi / 2)) * speed); - velo_ns = static_cast(sin_f32(DEG_TO_RAD(angle)) * speed); + velo_ew = static_cast(sin_f32(DEG_TO_RAD(angle) ) * speed); // East direction, is the projection from West -> East is directly sin(angle=Compas Bearing) , (90º is the max +1, EAST) max velo_EW + velo_ns = static_cast(sin_f32( (pi/2 - DEG_TO_RAD(angle) ) ) * speed); // North direction,is the projection of North = cos(angle=Compas Bearing), cos(angle)= sen(90-angle) (0º is the max +1 NORTH) max velo_NS - v_rate_coded = (v_rate / 64) + 1; + v_rate_coded = (v_rate / 64) + 1; //encoding vertical rate source. (Decoding, VR ft/min = (Decimal v_rate_value - 1)* 64) - velo_ew_abs = abs(velo_ew) + 1; - velo_ns_abs = abs(velo_ns) + 1; + velo_ew_abs = abs(velo_ew) + 1; // encoding Velo speed EW , when sign Direction is 0 (+): West->East, (-) 1: East->West + velo_ns_abs = abs(velo_ns) + 1; // encoding Velo speed NS , when sign Direction is 0 (+): South->North , (-) 1: North->South v_rate_coded_abs = abs(v_rate_coded); make_frame_adsb(frame, ICAO_address); - frame.push_byte((TC_AIRBORNE_VELO << 3) | 1); // Subtype: 1 (subsonic) + // Airborne velocities are all transmitted with Type Code 19 ( TC=19, using 5 bits ,TC=19 [Binary: 10011]), the following 3 bits are Subt-type Code ,SC= 1,2,3,4 + // SC Subtypes code 1 and 2 are used to report ground speeds of aircraft. (SC 3,4 to used to report true airspeed. SC 2,4 are for supersonic aircraft (not used in commercial airline). + frame.push_byte((TC_AIRBORNE_VELO << 3) | 1); // 1st byte , top 5 bits Type Code TC=19, and lower 3 bits (38-40 bits), SC=001 Subtype Code SC: 1 (subsonic) , + + // Message A, (ME bits from 14-35) , 22 bits = Sign ew(1 bit) + V_ew (10 bits) + Sign_ns (1 bit) + V_ns (10 bits) + // Vertical rate source bit VrSrc (ME bit 36) indicates source of the altitude measurements. GNSS altitude(0) / , barometric altitude(1). + // Vertical rate source direction,(ME bit 37) movement can be read from Svr bit , with 0 and 1 referring to climb and descent, respectively (ft/min) + // The encoded vertical rate value VR can be computed using message (ME bits 38 to 46). If the 9-bit block contains all zeros, the vertical rate information is not available. + // + Sign VrSrc (vert rate src) (1 bit)+ VrSrc (9 bits). frame.push_byte(((velo_ew < 0 ? 1 : 0) << 2) | (velo_ew_abs >> 8)); frame.push_byte(velo_ew_abs); frame.push_byte(((velo_ns < 0 ? 1 : 0) << 7) | (velo_ns_abs >> 3)); diff --git a/firmware/common/sine_table.hpp b/firmware/common/sine_table.hpp index a585ad62..8431bb29 100644 --- a/firmware/common/sine_table.hpp +++ b/firmware/common/sine_table.hpp @@ -37,7 +37,7 @@ print(v) */ constexpr uint16_t sine_table_f32_period = 256; // periode is 256 . means sine_table_f32[0]= sine_table_f32[0+256], sine_table_f32[1]=sine_table_f32[1+256] (those two added manualy) -// Then table has 257 values , [0,..255] + [256] and [257], those two are used when we interpolate[255] with [255+1], and [256] with [256+1] +// Then table has 258 values ,256:[0,..255] + [256] and [257], those two are used when we interpolate[255] with [255+1], and [256] with [256+1] // [256] index is needed in the function sin_f32() when we are inputing very small radian values , example , sin_f32((-1e-14) in radians) static constexpr std::array sine_table_f32{