diff --git a/firmware/application/apps/ui_sonde.cpp b/firmware/application/apps/ui_sonde.cpp index 16506b3f..e61139ac 100644 --- a/firmware/application/apps/ui_sonde.cpp +++ b/firmware/application/apps/ui_sonde.cpp @@ -54,7 +54,7 @@ SondeView::SondeView(NavigationView& nav) { }); field_frequency.set_value(target_frequency_); - field_frequency.set_step(10000); + field_frequency.set_step(500); //euquiq: was 10000, but we are using this for fine-tunning field_frequency.on_change = [this](rf::Frequency f) { set_target_frequency(f); field_frequency.set_value(f); @@ -86,12 +86,12 @@ SondeView::SondeView(NavigationView& nav) { button_see_map.on_select = [this, &nav](Button&) { nav.push( - "", - altitude, + sonde_id, + gps_info.alt, GeoPos::alt_unit::METERS, - latitude, - longitude, - 0); + gps_info.lat, + gps_info.lon, + 999); //set a dummy heading out of range to draw a cross...probably not ideal? }; logger = std::make_unique(); @@ -113,16 +113,15 @@ void SondeView::on_packet(const sonde::Packet& packet) { //const auto hex_formatted = packet.symbols_formatted(); text_signature.set(packet.type_string()); - text_serial.set(packet.serial_number()); + sonde_id = packet.serial_number(); //used also as tag on the geomap + text_serial.set(sonde_id); text_voltage.set(unit_auto_scale(packet.battery_voltage(), 2, 3) + "V"); + + gps_info = packet.get_GPS_data(); - altitude = packet.GPS_altitude(); - latitude = packet.GPS_latitude(); - longitude = packet.GPS_longitude(); - - geopos.set_altitude(altitude); - geopos.set_lat(latitude); - geopos.set_lon(longitude); + geopos.set_altitude(gps_info.alt); + geopos.set_lat(gps_info.lat); + geopos.set_lon(gps_info.lon); if (logger && logging) { logger->on_packet(packet); diff --git a/firmware/application/apps/ui_sonde.hpp b/firmware/application/apps/ui_sonde.hpp index 5dc7fe86..9e7743b1 100644 --- a/firmware/application/apps/ui_sonde.hpp +++ b/firmware/application/apps/ui_sonde.hpp @@ -65,11 +65,10 @@ public: private: std::unique_ptr logger { }; - uint32_t target_frequency_ { 402000000 }; + uint32_t target_frequency_ { 402700000 }; bool logging { false }; - int32_t altitude { 0 }; - float latitude { 0 }; - float longitude { 0 }; + sonde::GPS_data gps_info; + std::string sonde_id; Labels labels { { { 0 * 8, 2 * 16 }, "Signature:", Color::light_grey() }, diff --git a/firmware/baseband/proc_sonde.hpp b/firmware/baseband/proc_sonde.hpp index de15fff0..92e53580 100644 --- a/firmware/baseband/proc_sonde.hpp +++ b/firmware/baseband/proc_sonde.hpp @@ -140,7 +140,8 @@ private: } }; PacketBuilder packet_builder_fsk_4800_Vaisala { - { 0b00001000011011010101001110001000, 32, 1 }, + { 0b00001000011011010101001110001000, 32, 1 }, //euquiq Header detects 4 of 8 bytes 0x10B6CA11 /this is in raw format) (these bits are not passed at the beginning of packet) + //{ 0b0000100001101101010100111000100001000100011010010100100000011111, 64, 1 }, //euquiq whole header detection would be 8 bytes. { }, { 320 * 8 }, [this](const baseband::Packet& packet) { diff --git a/firmware/common/sonde_packet.cpp b/firmware/common/sonde_packet.cpp index 22f54793..b1c22430 100644 --- a/firmware/common/sonde_packet.cpp +++ b/firmware/common/sonde_packet.cpp @@ -22,9 +22,25 @@ #include "sonde_packet.hpp" #include "string_format.hpp" +#include +//#include namespace sonde { +//Defines for Vaisala RS41, from https://github.com/rs1729/RS/blob/master/rs41/rs41sg.c +#define MASK_LEN 64 +#define pos_FrameNb 0x37 //0x03B // 2 byte +#define pos_SondeID 0x39 //0x03D // 8 byte +#define pos_Voltage 0x041 //0x045 // 3 bytes (but first one is the important one) voltage x 10 ie: 26 = 2.6v +#define pos_CalData 0x04E //0x052 // 1 byte, counter 0x00..0x32 +#define pos_GPSweek 0x091 //0x095 // 2 byte +#define pos_GPSTOW 0x093 //0x097 // 4 byte +#define pos_GPSecefX 0x110 //0x114 // 4 byte +#define pos_GPSecefY 0x114 //0x118 // 4 byte (not actually used since Y and Z are following X, and grabbed in that same loop) +#define pos_GPSecefZ 0x118 //0x11C // 4 byte (same as Y) + +#define PI 3.1415926535897932384626433832795 //3.1416 //(3.1415926535897932384626433832795) + Packet::Packet( const baseband::Packet& packet, const Type type @@ -60,37 +76,65 @@ Packet::Type Packet::type() const { return type_; } -/*uint8_t Packet::vaisala_descramble(const uint32_t pos) { - return reader_raw.read(pos * 8, 8) ^ vaisala_mask[pos & 63]; -};*/ +//euquiq here: +//RS41SG 320 bits header, 320bytes frame (or more if it is an "extended frame") +//The raw data is xor-scrambled with the values in the 64 bytes vaisala_mask (see.hpp) -uint32_t Packet::GPS_altitude() const { - if ((type_ == Type::Meteomodem_M10) || (type_ == Type::Meteomodem_M2K2)) - return (reader_bi_m.read(22 * 8, 32) / 1000) - 48; - else if (type_ == Type::Vaisala_RS41_SG) { - /*uint32_t altitude_ecef = 0; - for (uint32_t i = 0; i < 4; i++) - altitude_ecef = (altitude_ecef << 8) + vaisala_descramble(0x11C + i);*/ - // TODO: and a bunch of maths (see ecef2elli() from RS1729) - return 0; - } else - return 0; // Unknown -} -float Packet::GPS_latitude() const { - if ((type_ == Type::Meteomodem_M10) || (type_ == Type::Meteomodem_M2K2)) - return reader_bi_m.read(14 * 8, 32) / ((1ULL << 32) / 360.0); - //else if (type_ == Type::Vaisala_RS41_SG) - // return vaisala_descramble(); - else - return 0; // Unknown -} +uint8_t Packet::vaisala_descramble(const uint32_t pos) const { + //return reader_raw.read(pos * 8, 8) ^ vaisala_mask[pos & 63]; + // packet_[i]; its a bit; packet_.size the total (should be 2560 bits) + uint8_t value = 0; + for (uint8_t i = 0; i < 8; i++) + value = (value << 1) | packet_[(pos * 8) + (7 -i)]; //get the byte from the bits collection -float Packet::GPS_longitude() const { - if ((type_ == Type::Meteomodem_M10) || (type_ == Type::Meteomodem_M2K2)) - return reader_bi_m.read(18 * 8, 32) / ((1ULL << 32) / 360.0); - else - return 0; // Unknown + //packetReader reader { packet_ }; //This works just as above. + //value = reader.read(pos * 8,8); + //shift pos because first 4 bytes are consumed by proc_sonde in finding the vaisala signature + uint32_t mask_pos = pos + 4; + value = value ^ vaisala_mask[mask_pos % MASK_LEN]; //descramble with the xor pseudorandom table + return value; +}; + +GPS_data Packet::get_GPS_data() const { + GPS_data result; + if ((type_ == Type::Meteomodem_M10) || (type_ == Type::Meteomodem_M2K2)) { + + result.alt = (reader_bi_m.read(22 * 8, 32) / 1000) - 48; + result.lat = reader_bi_m.read(14 * 8, 32) / ((1ULL << 32) / 360.0); + result.lon = reader_bi_m.read(18 * 8, 32) / ((1ULL << 32) / 360.0); + + } else if (type_ == Type::Vaisala_RS41_SG) { + + uint8_t XYZ_bytes[4]; + int32_t XYZ; // 32bit + double_t X[3]; + for (int32_t k = 0; k < 3; k++) { //Get X,Y,Z ECEF position from GPS + for (int32_t i = 0; i < 4; i++) //each one is 4 bytes (32 bits) + XYZ_bytes[i] = vaisala_descramble(pos_GPSecefX + (4*k) + i); + memcpy(&XYZ, XYZ_bytes, 4); + X[k] = XYZ / 100.0; + } + + double_t a = 6378137.0; + double_t b = 6356752.31424518; + double_t e = sqrt( (a*a - b*b) / (a*a) ); + double_t ee = sqrt( (a*a - b*b) / (b*b) ); + + double_t lam = atan2( X[1] , X[0] ); + double_t p = sqrt( X[0]*X[0] + X[1]*X[1] ); + double_t t = atan2( X[2]*a , p*b ); + double_t phi = atan2( X[2] + ee*ee * b * sin(t)*sin(t)*sin(t) , + p - e*e * a * cos(t)*cos(t)*cos(t) ); + + double_t R = a / sqrt( 1 - e*e*sin(phi)*sin(phi) ); + + result.alt = p / cos(phi) - R; + result.lat = phi*180/PI; + result.lon = lam*180/PI; + + } + return result; } uint32_t Packet::battery_voltage() const { @@ -98,8 +142,13 @@ uint32_t Packet::battery_voltage() const { return (reader_bi_m.read(69 * 8, 8) + (reader_bi_m.read(70 * 8, 8) << 8)) * 1000 / 150; else if (type_ == Type::Meteomodem_M2K2) return reader_bi_m.read(69 * 8, 8) * 66; // Actually 65.8 - else + else if (type_ == Type::Vaisala_RS41_SG) { + uint32_t voltage = vaisala_descramble(pos_Voltage) * 100; //byte 69 = voltage * 10 (check if this value needs to be multiplied) + return voltage; + } + else { return 0; // Unknown + } } std::string Packet::type_string() const { @@ -127,12 +176,33 @@ std::string Packet::serial_number() const { to_string_dec_uint(reader_bi_m.read(93 * 8 + 24, 3), 1) + to_string_dec_uint(reader_bi_m.read(93 * 8 + 27, 13), 4, '0'); - } else + } else if(type() == Type::Vaisala_RS41_SG) { + std::string serial_id = ""; + uint8_t achar; + for (uint8_t i=0; i<8; i++) { //euquiq: Serial ID is 8 bytes long, each byte a char + achar = vaisala_descramble(pos_SondeID + i); + if (achar < 32 || achar > 126) return "?"; //Maybe there are ids with less than 8 bytes and this is not OK. + serial_id += (char)achar; + } + return serial_id; + } else return "?"; } FormattedSymbols Packet::symbols_formatted() const { - return format_symbols(decoder_); + if (type() == Type::Vaisala_RS41_SG) { //Euquiq: now we distinguish different types + uint32_t bytes = packet_.size() / 8; //Need the byte amount, which if full, it SHOULD be 320 size() should return 2560 + std::string hex_data; + std::string hex_error; + hex_data.reserve(bytes * 2); //2 hexa chars per byte + hex_error.reserve(1); + for (uint32_t i=0; i < bytes; i++) //log will show the packet starting on the last 4 bytes from signature 93DF1A60 + hex_data += to_string_hex(vaisala_descramble(i),2); + return { hex_data, hex_error }; + + } else { + return format_symbols(decoder_); + } } bool Packet::crc_ok() const { diff --git a/firmware/common/sonde_packet.hpp b/firmware/common/sonde_packet.hpp index 4ecbc08a..746d42e3 100644 --- a/firmware/common/sonde_packet.hpp +++ b/firmware/common/sonde_packet.hpp @@ -32,6 +32,12 @@ namespace sonde { + struct GPS_data { + uint32_t alt { 0 }; + float lat { 0 }; + float lon { 0 }; + }; + class Packet { public: enum class Type : uint32_t { @@ -41,7 +47,7 @@ public: Meteomodem_M2K2 = 3, Vaisala_RS41_SG = 4, }; - + Packet(const baseband::Packet& packet, const Type type); size_t length() const; @@ -56,9 +62,7 @@ public: std::string serial_number() const; uint32_t battery_voltage() const; - uint32_t GPS_altitude() const; - float GPS_latitude() const; - float GPS_longitude() const; + GPS_data get_GPS_data() const; FormattedSymbols symbols_formatted() const; @@ -75,17 +79,20 @@ private: 0xD0, 0xBC, 0xB4, 0xB6, 0x06, 0xAA, 0xF4, 0x23, 0x78, 0x6E, 0x3B, 0xAE, 0xBF, 0x7B, 0x4C, 0xC1 }; + + GPS_data ecef_to_gps() const; - //uint8_t vaisala_descramble(const uint32_t pos); + uint8_t vaisala_descramble(uint32_t pos) const; const baseband::Packet packet_; const BiphaseMDecoder decoder_; const FieldReader reader_bi_m; Type type_; + using packetReader = FieldReader; //baseband::Packet instead of BiphaseMDecoder bool crc_ok_M10() const; }; } /* namespace sonde */ -#endif/*__SONDE_PACKET_H__*/ +#endif/*__SONDE_PACKET_H__*/ \ No newline at end of file