TheArchive/mayhem-firmware
1
0
Fork 0
mirror of https://github.com/portapack-mayhem/mayhem-firmware.git synced 2025-08-25 19:29:55 +00:00
Code Issues Packages Projects Releases Wiki Activity

ADSB position decoding

Browse Source 
Date and time string format function
Binary update
This commit is contained in:
furrtek
2017-08-17 12:56:47 +01:00
parent 9d902bc224
commit 2628f9c03d
15 changed files with 255 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
firmware/common/adsb.cpp
View File

@@ -138,11 +138,11 @@ void encode_frame_squawk(ADSBFrame& frame, const uint32_t squawk) {
}
float cpr_mod(float a, float b) {
return a - (b * floor(a / b));
return a - (b * floor(a / b));
}
int cpr_NL(float lat) {
if (lat < 0)
if (lat < 0)
lat = -lat; // Symmetry
for (size_t c = 0; c < 58; c++) {
@@ -150,7 +150,7 @@ int cpr_NL(float lat) {
return 59 - c;
}
return 1;
return 1;
}
int cpr_N(float lat, int is_odd) {
@@ -162,6 +162,10 @@ int cpr_N(float lat, int is_odd) {
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) {
@@ -203,35 +207,76 @@ void encode_frame_pos(ADSBFrame& frame, const uint32_t ICAO_address, const int32
frame.make_CRC();
}
// Decoding method (from dump1090):
// index int j = floor(((59 * latcprE - 60 * latcprO) / 131072) + 0.50)
// latE = DlatE * (cpr_mod(j, 60) + (latcprE / 131072))
// latO = DlatO * (cpr_mod(j, 59) + (latcprO / 131072))
// if latE >= 270 -> latE -= 360
// if latO >= 270 -> latO -= 360
// if (cpr_NL(latE) != cpr_NL(latO)) return;
// int ni = cpr_N(latE ,0);
// int m = floor((((loncprE * (cpr_NL(latE) - 1)) - (loncprO * cpr_NL(latE))) / 131072) + 0.5)
// lon = cpr_Dlon(latE, 0) * (cpr_mod(m, ni) + loncprE / 131072);
// lat = latE;
// ... or ...
// int ni = cpr_N(latO ,0);
// int m = floor((((loncprE * (cpr_NL(latO) - 1)) - (loncprO * cpr_NL(latO))) / 131072) + 0.5)
// lon = cpr_Dlon(latO, 0) * (cpr_mod(m, ni) + loncprO / 131072);
// lat = latO;
// ... and ...
// if (lon > 180) lon -= 360;
// Only altitude is decoded for now
uint32_t decode_frame_pos(ADSBFrame& frame) {
uint8_t * raw_data = frame.get_raw_data();
// 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;
int ni;
adsb_pos position { false, 0, 0, 0 };
// Q-bit is present
if (raw_data[5] & 1)
return ((((raw_data[5] >> 1) << 4) | ((raw_data[6] & 0xF0) >> 4)) * 25) - 1000;
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 0;
// 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];
// Compute latitude index
float j = floor((((59.0 * latcprE) - (60.0 * latcprO)) / 131072.0) + 0.5);
latE = (360.0 / 60.0) * (cpr_mod(j, 60) + (latcprE / 131072.0));
latO = (360.0 / 59.0) * (cpr_mod(j, 59) + (latcprO / 131072.0));
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 frame
ni = cpr_N(latE, 0);
Dlon = 360.0 / ni;
m = floor((((loncprE * (cpr_NL(latE) - 1)) - (loncprO * cpr_NL(latE))) / 131072.0) + 0.5);
position.longitude = Dlon * (cpr_mod(m, ni) + loncprE / 131072.0);
position.latitude = latE;
} else {
// Use odd frame
ni = cpr_N(latO, 1);
Dlon = 360.0 / ni;
m = floor((((loncprE * (cpr_NL(latO) - 1)) - (loncprO * cpr_NL(latO))) / 131072.0) + 0.5);
position.longitude = Dlon * (cpr_mod(m, ni) + loncprO / 131072.0);
position.latitude = latO;
}
if (position.longitude > 180) position.longitude -= 360;
position.valid = true;
return position;
}
// speed is in knots

10
firmware/common/adsb.hpp
View File

@@ -49,6 +49,13 @@ enum data_selector {
BDS_HEADING = 0x60
};
struct adsb_pos {
bool valid;
float latitude;
float longitude;
int32_t altitude;
};
const float adsb_lat_lut[58] = {
10.47047130, 14.82817437, 18.18626357, 21.02939493,
23.54504487, 25.82924707, 27.93898710, 29.91135686,
@@ -74,7 +81,8 @@ std::string decode_frame_id(ADSBFrame& frame);
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 decode_frame_pos(ADSBFrame& frame);
adsb_pos decode_frame_pos(ADSBFrame& frame_even, ADSBFrame& frame_odd);
void encode_frame_velo(ADSBFrame& frame, const uint32_t ICAO_address, const uint32_t speed,
const float angle, const int32_t v_rate);

22
firmware/common/adsb_frame.hpp
View File

@@ -44,6 +44,13 @@ public:
uint32_t get_ICAO_address() {
return (raw_data[1] << 16) + (raw_data[2] << 8) + raw_data[3];
}
void set_rx_timestamp(uint32_t timestamp) {
rx_timestamp = timestamp;
}
uint32_t get_rx_timestamp() {
return rx_timestamp;
}
void clear() {
index = 0;
@@ -57,8 +64,8 @@ public:
raw_data[index++] = byte;
}
uint8_t * get_raw_data() {
return raw_data;
uint8_t * get_raw_data() const {
return (uint8_t* const)raw_data;
}
void make_CRC() {
@@ -73,18 +80,23 @@ public:
bool check_CRC() {
uint32_t computed_CRC = compute_CRC();
if (raw_data[11] != ((computed_CRC >> 16) & 0xFF)) return false;
if (raw_data[12] != ((computed_CRC >> 8) & 0xFF)) return false;
if (raw_data[13] != (computed_CRC & 0xFF)) return false;
if ((raw_data[11] != ((computed_CRC >> 16) & 0xFF)) ||
(raw_data[12] != ((computed_CRC >> 8) & 0xFF)) ||
(raw_data[13] != (computed_CRC & 0xFF))) return false;
return true;
}
bool empty() {
return (index == 0);
}
private:
static const uint8_t adsb_preamble[16];
static const char icao_id_lut[65];
alignas(4) uint8_t index { 0 };
alignas(4) uint8_t raw_data[14] { }; // 112 bits at most
uint32_t rx_timestamp { };
uint32_t compute_CRC() {
uint8_t adsb_crc[14] = { 0 }; // Temp buffer

11
firmware/common/ui.cpp
View File

@@ -26,6 +26,17 @@
namespace ui {
Color term_colors[8] = {
Color::black(),
Color::red(),
Color::green(),
Color::yellow(),
Color::blue(),
Color::magenta(),
Color::cyan(),
Color::white()
};
bool Rect::contains(const Point p) const {
return (p.x() >= left()) && (p.y() >= top()) &&
(p.x() < right()) && (p.y() < bottom());

16
firmware/common/ui.hpp
View File

@@ -77,7 +77,7 @@ struct Color {
return { 255, 175, 0 };
}
static constexpr Color dark_orange() {
return { 127, 88, 0 };
return { 127, 88, 0 };
}
static constexpr Color yellow() {
@@ -102,7 +102,11 @@ struct Color {
}
static constexpr Color cyan() {
return { 0, 255, 255 };
return { 0, 255, 255 };
}
static constexpr Color magenta() {
return { 255, 0, 255 };
}
static constexpr Color white() {
@@ -113,17 +117,19 @@ struct Color {
return { 127, 127, 127 };
}
static constexpr Color grey() {
return { 91, 91, 91 };
return { 91, 91, 91 };
}
static constexpr Color dark_grey() {
return { 63, 63, 63 };
return { 63, 63, 63 };
}
static constexpr Color purple() {
return { 204, 0, 102 };
return { 204, 0, 102 };
}
};
extern Color term_colors[8];
struct ColorRGB888 {
uint8_t r;
uint8_t g;

22
firmware/common/ui_painter.cpp
View File

@@ -45,14 +45,26 @@ int Painter::draw_char(const Point p, const Style& style, const char c) {
int Painter::draw_string(Point p, const Font& font, const Color foreground,
const Color background, const std::string text) {
bool escape = false;
size_t width = 0;
Color pen = foreground;
for(const auto c : text) {
const auto glyph = font.glyph(c);
display.draw_glyph(p, glyph, foreground, background);
const auto advance = glyph.advance();
p += advance;
width += advance.x();
if (escape) {
pen = term_colors[c & 7];
if (!c) pen = foreground;
escape = false;
} else {
if (c == '\x1B') {
escape = true;
} else {
const auto glyph = font.glyph(c);
display.draw_glyph(p, glyph, pen, background);
const auto advance = glyph.advance();
p += advance;
width += advance.x();
}
}
}
return width;
}

7
firmware/common/ui_widget.cpp
View File

@@ -546,12 +546,7 @@ void Console::write(std::string message) {
for (const auto c : message) {
if (escape) {
if (c == '\x01')
pen_color = ui::Color::red();
else if (c == '\x02')
pen_color = ui::Color::green();
else if (c == '\x03')
pen_color = ui::Color::blue();
pen_color = term_colors[c & 7];
escape = false;
} else {
if (c == '\n') {