mirror of
https://github.com/eried/portapack-mayhem.git
synced 2024-09-20 16:15:51 +00:00
778111d466
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
393 lines
13 KiB
C++
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 */
|