portapack-mayhem/firmware/common/adsb.cpp
heurist1 778111d466 Updates to put the multiple aircraft on the map
Also manage the CPU in on the one second tick to keep GUI responsive
Some other small edits that fix minor problems from my previous checkins
2023-02-28 20:19:23 +00:00

393 lines
13 KiB
C++

/*
* Copyright (C) 2014 Jared Boone, ShareBrained Technology, Inc.
* Copyright (C) 2016 Furrtek
*
* This file is part of PortaPack.
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; see the file COPYING. If not, write to
* the Free Software Foundation, Inc., 51 Franklin Street,
* Boston, MA 02110-1301, USA.
*/
#include "adsb.hpp"
#include "sine_table.hpp"
#include "utility.hpp"
#include <math.h>
namespace adsb {
void make_frame_adsb(ADSBFrame& frame, const uint32_t ICAO_address) {
frame.clear();
frame.push_byte((DF_ADSB << 3) | 5); // DF and CA
frame.push_byte(ICAO_address >> 16);
frame.push_byte(ICAO_address >> 8);
frame.push_byte(ICAO_address & 0xFF);
}
void encode_frame_id(ADSBFrame& frame, const uint32_t ICAO_address, const std::string& callsign) {
std::string callsign_formatted(8, '_');
uint64_t callsign_coded = 0;
uint32_t c, s;
char ch;
make_frame_adsb(frame, ICAO_address);
frame.push_byte(TC_IDENT << 3); // No aircraft category
// Translate and encode callsign
for (c = 0; c < 8; c++) {
ch = callsign[c];
for (s = 0; s < 64; s++)
if (ch == icao_id_lut[s]) break;
if (s == 64) {
ch = ' '; // Invalid character
s = 32;
}
callsign_coded <<= 6;
callsign_coded |= s;
//callsign[c] = ch;
}
// Insert callsign in frame
for (c = 0; c < 6; c++)
frame.push_byte((callsign_coded >> ((5 - c) * 8)) & 0xFF);
frame.make_CRC();
}
std::string decode_frame_id(ADSBFrame& frame) {
std::string callsign = "";
uint8_t * raw_data = frame.get_raw_data();
uint64_t callsign_coded = 0;
uint32_t c;
// Frame bytes to long
for (c = 5; c < 11; c++) {
callsign_coded <<= 8;
callsign_coded |= raw_data[c];
}
// Long to 6-bit characters
for (c = 0; c < 8; c++) {
callsign.append(1, icao_id_lut[(callsign_coded >> 42) & 0x3F]);
callsign_coded <<= 6;
}
return callsign;
}
/*void generate_frame_emergency(ADSBFrame& frame, const uint32_t ICAO_address, const uint8_t code) {
make_frame_mode_s(frame, ICAO_address);
frame.push_byte((28 << 3) + 1); // TC = 28 (Emergency), subtype = 1 (Emergency)
frame.push_byte(code << 5);
frame.make_CRC();
}*/
void encode_frame_squawk(ADSBFrame& frame, const uint32_t squawk) {
uint32_t squawk_coded;
frame.clear();
frame.push_byte(DF_EHS_SQUAWK << 3); // DF
frame.push_byte(0);
// 12 11 10 9 8 7 6 5 4 3 2 1 0
// 31 30 29 28 27 26 25 24 23 22 21 20 19
// D4 B4 D2 B2 D1 B1 __ A4 C4 A2 C2 A1 C1
// ABCD = code (octal, 0000~7777)
// FEDCBA9876543210
// xAAAxBBBxCCCxDDD
// x421x421x421x421
squawk_coded = ((squawk << 10) & 0x1000) | // D4
((squawk << 1) & 0x0800) | // B4
((squawk << 9) & 0x0400) | // D2
((squawk << 0) & 0x0200) | // B2
((squawk << 8) & 0x0100) | // D1
((squawk >> 1) & 0x0080) | // B1
((squawk >> 9) & 0x0020) | // A4
((squawk >> 2) & 0x0010) | // C4
((squawk >> 10) & 0x0008) | // A2
((squawk >> 3) & 0x0004) | // C2
((squawk >> 11) & 0x0002) | // A1
((squawk >> 4) & 0x0001); // C1
frame.push_byte(squawk_coded >> 5);
frame.push_byte(squawk_coded << 3);
frame.make_CRC();
}
float cpr_mod(float a, float b) {
return a - (b * floor(a / b));
}
int cpr_NL_precise(float lat) {
return (int) floor(2 * PI / acos(1 - ((1 - cos(PI / (2 * NZ))) / pow(cos(PI * lat / 180), 2))));
}
int cpr_NL_approx(float lat) {
if (lat < 0)
lat = -lat; // Symmetry
for (size_t c = 0; c < 58; c++) {
if (lat < adsb_lat_lut[c])
return 59 - c;
}
return 1;
}
int cpr_NL(float lat) {
// TODO prove that the approximate function is good
// enough for the precision we need. Uncomment if
// that is true. No performance penalty was noticed
// from testing, but if you find it might be an issue,
// switch to cpr_NL_approx() instead:
//return cpr_NL_approx(lat);
return cpr_NL_precise(lat);
}
int cpr_N(float lat, int is_odd) {
int nl = cpr_NL(lat) - is_odd;
if (nl < 1)
nl = 1;
return nl;
}
float cpr_Dlon(float lat, int is_odd) {
return 360.0 / cpr_N(lat, is_odd);
}
void encode_frame_pos(ADSBFrame& frame, const uint32_t ICAO_address, const int32_t altitude,
const float latitude, const float longitude, const uint32_t time_parity) {
uint32_t altitude_coded;
uint32_t lat, lon;
float delta_lat, yz, rlat, delta_lon, xz;
make_frame_adsb(frame, ICAO_address);
frame.push_byte(TC_AIRBORNE_POS << 3); // Bits 2~1: Surveillance Status, bit 0: NICsb
altitude_coded = (altitude + 1000) / 25; // 25ft precision, insert Q-bit (1)
altitude_coded = ((altitude_coded & 0x7F0) << 1) | 0x10 | (altitude_coded & 0x0F);
frame.push_byte(altitude_coded >> 4); // Top-most altitude bits
// CPR encoding
// Info from: http://antena.fe.uni-lj.si/literatura/Razno/Avionika/modes/CPRencoding.pdf
delta_lat = 360.0 / ((4.0 * NZ) - time_parity); // NZ = 15
yz = floor(CPR_MAX_VALUE * (cpr_mod(latitude, delta_lat) / delta_lat) + 0.5);
rlat = delta_lat * ((yz / CPR_MAX_VALUE) + floor(latitude / delta_lat));
if ((cpr_NL(rlat) - time_parity) > 0)
delta_lon = 360.0 / cpr_N(rlat, time_parity);
else
delta_lon = 360.0;
xz = floor(CPR_MAX_VALUE * (cpr_mod(longitude, delta_lon) / delta_lon) + 0.5);
lat = cpr_mod(yz, CPR_MAX_VALUE);
lon = cpr_mod(xz, CPR_MAX_VALUE);
frame.push_byte((altitude_coded << 4) | ((uint32_t)time_parity << 2) | (lat >> 15)); // T = 0
frame.push_byte(lat >> 7);
frame.push_byte((lat << 1) | (lon >> 16));
frame.push_byte(lon >> 8);
frame.push_byte(lon);
frame.make_CRC();
}
// Decoding method from dump1090
adsb_pos decode_frame_pos(ADSBFrame& frame_even, ADSBFrame& frame_odd) {
uint8_t * raw_data;
uint32_t latcprE, latcprO, loncprE, loncprO;
float latE, latO, m, Dlon, cpr_lon_odd, cpr_lon_even, cpr_lat_odd, cpr_lat_even;
int ni;
adsb_pos position { false, 0, 0, 0 };
uint32_t time_even = frame_even.get_rx_timestamp();
uint32_t time_odd = frame_odd.get_rx_timestamp();
uint8_t * frame_data_even = frame_even.get_raw_data();
uint8_t * frame_data_odd = frame_odd.get_raw_data();
// Return most recent altitude
if (time_even > time_odd)
raw_data = frame_data_even;
else
raw_data = frame_data_odd;
// Q-bit must be present
if (raw_data[5] & 1)
position.altitude = ((((raw_data[5] & 0xFE) << 3) | ((raw_data[6] & 0xF0) >> 4)) * 25) - 1000;
// Position
latcprE = ((frame_data_even[6] & 3) << 15) | (frame_data_even[7] << 7) | (frame_data_even[8] >> 1);
loncprE = ((frame_data_even[8] & 1) << 16) | (frame_data_even[9] << 8) | frame_data_even[10];
latcprO = ((frame_data_odd[6] & 3) << 15) | (frame_data_odd[7] << 7) | (frame_data_odd[8] >> 1);
loncprO = ((frame_data_odd[8] & 1) << 16) | (frame_data_odd[9] << 8) | frame_data_odd[10];
// Calculate the coefficients
cpr_lon_even = loncprE / CPR_MAX_VALUE;
cpr_lon_odd = loncprO / CPR_MAX_VALUE;
cpr_lat_odd = latcprO / CPR_MAX_VALUE;
cpr_lat_even = latcprE / CPR_MAX_VALUE;
// Compute latitude index
float j = floor(((59.0 * cpr_lat_even) - (60.0 * cpr_lat_odd)) + 0.5);
latE = (360.0 / 60.0) * (cpr_mod(j, 60) + cpr_lat_even);
latO = (360.0 / 59.0) * (cpr_mod(j, 59) + cpr_lat_odd);
if (latE >= 270) latE -= 360;
if (latO >= 270) latO -= 360;
// Both frames must be in the same latitude zone
if (cpr_NL(latE) != cpr_NL(latO))
return position;
// Compute longitude
if (time_even > time_odd) {
// Use even frame2
ni = cpr_N(latE, 0);
Dlon = 360.0 / ni;
m = floor((cpr_lon_even * (cpr_NL(latE) - 1)) - (cpr_lon_odd * cpr_NL(latE)) + 0.5);
position.longitude = Dlon * (cpr_mod(m, ni) + cpr_lon_even);
position.latitude = latE;
} else {
// Use odd frame
ni = cpr_N(latO, 1);
Dlon = 360.0 / ni;
m = floor((cpr_lon_even * (cpr_NL(latO) - 1)) - (cpr_lon_odd * cpr_NL(latO)) + 0.5);
position.longitude = Dlon * (cpr_mod(m, ni) + cpr_lon_odd);
position.latitude = latO;
}
if (position.longitude >= 180) position.longitude -= 360;
position.valid = true;
return position;
}
// speed is in knots
// vertical rate is in ft/min
void encode_frame_velo(ADSBFrame& frame, const uint32_t ICAO_address, const uint32_t speed,
const float angle, const int32_t v_rate) {
int32_t velo_ew, velo_ns;
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<int32_t>(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<int32_t>(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_abs = (abs(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; // 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
make_frame_adsb(frame, ICAO_address);
// 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));
frame.push_byte((velo_ns_abs << 5) | ((v_rate < 0 ? 1 : 0) << 3) | (v_rate_coded_abs >> 6)); // VrSrc = 0
frame.push_byte(v_rate_coded_abs << 2);
frame.push_byte(0);
frame.make_CRC();
}
// Decoding method from dump1090
adsb_vel decode_frame_velo(ADSBFrame& frame){
adsb_vel velo {false, 0, 0, 0};
uint8_t * frame_data = frame.get_raw_data();
uint8_t velo_type = frame.get_msg_sub();
if(velo_type >= 1 && velo_type <= 4){ //vertical rate is always present
velo.v_rate = (((frame_data[8] & 0x07 ) << 6) | ((frame_data[9] >> 2) - 1)) * 64;
if((frame_data[8] & 0x8) >> 3) velo.v_rate *= -1; //check v_rate sign
}
if(velo_type == 1 || velo_type == 2){ //Ground Speed
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]&0x04) velo_ew *= -1; //check ew direction sign
if(frame_data[7]&0x80) velo_ns *= -1; //check ns direction sign
velo.speed = fast_int_magnitude(velo_ns,velo_ew);
if(velo.speed){
//calculate heading in degrees from ew/ns velocities
int16_t heading_temp = (int16_t)(int_atan2(velo_ew,velo_ns)); // Nearest degree
// We don't want negative values but a 0-360 scale.
if (heading_temp < 0) heading_temp += 360.0;
velo.heading = (uint16_t)heading_temp;
}
}else if(velo_type == 3 || velo_type == 4){ //Airspeed
velo.valid = frame_data[5] & (1<<2);
velo.heading = ((((frame_data[5] & 0x03)<<8) | frame_data[6]) * 45) << 7;
}
return velo;
}
} /* namespace adsb */