Skip to content

Commit

Permalink
Merge GLONASS into tightly (GPS+SBAS+QZSS)
Browse files Browse the repository at this point in the history
  • Loading branch information
@fenrir-naru
fenrir-naru committed Mar 28, 2024
2 parents e364460 + b37e08c commit e2b2097
Show file tree
Hide file tree
Showing 12 changed files with 3,382 additions and 13 deletions.
9 changes: 9 additions & 0 deletions tool/INS_GPS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -209,6 +209,7 @@ struct QuaternionData_TypeMapper<float_sylph_t> {
#include "navigation/INS_GPS_Synchronization.h"
#include "navigation/INS_GPS_Debug.h"
#include "navigation/GPS.h"
#include "navigation/GLONASS.h"

#include "navigation/WGS84.h"
#include "navigation/MagneticField.h"
Expand Down Expand Up @@ -1991,6 +1992,7 @@ class StreamProcessor
if(bytes > sizeof(packet.subframe.buffer)){return;}
observer.inspect(packet.subframe.buffer, (packet.subframe.bytes = bytes), 6 + 8);
if((G_Observer_t::u8_t)observer[6 + 2] != 0){return;} // TODO reserved1, sigID? (0:L1C/A, 4:L2CM?)
packet.subframe.glonass_freq_ch = -7 + (int)(G_Observer_t::u8_t)observer[6 + 3];
check_subframeX(
(G_Observer_t::u8_t)observer[6 + 0],
(G_Observer_t::u8_t)observer[6 + 1],
Expand Down Expand Up @@ -2058,6 +2060,13 @@ class StreamProcessor
dst.insert(std::make_pair(signal->signal_strength, observer[6 + 42 + (32 * i)]));
observer.inspect(buf, 2, 6 + 40 + (32 * i));
dst.insert(std::make_pair(signal->lock_sec, 1E-3 * le_char2_2_num<G_Observer_t::u16_t>(*buf)));

if(gnssID == G_Observer_t::gnss_svid_t::GLONASS){
int freq_ch(-7 + (int)observer[6 + 39 + (32 * i)]);
dst.insert(std::make_pair(
items_t::L1_FREQUENCY,
GLONASS_SpaceNode<float_sylph_t>::L1_frequency(freq_ch)));
}
}

packet_raw_latest.update_measurement(current, measurement);
Expand Down
71 changes: 70 additions & 1 deletion tool/INS_GPS/GNSS_Data.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include "navigation/GPS.h"
#include "navigation/SBAS.h"
#include "navigation/QZSS.h"
#include "navigation/GLONASS.h"

template <class FloatT>
struct GNSS_Data {
Expand All @@ -55,6 +56,7 @@ struct GNSS_Data {
unsigned int sv_number;
unsigned int bytes;
typename observer_t::v8_t buffer[40];
int glonass_freq_ch;
} subframe;

typedef GPS_Time<FloatT> gps_time_t;
Expand All @@ -65,6 +67,8 @@ struct GNSS_Data {
gps_t *gps;
typedef SBAS_SpaceNode<FloatT> sbas_t;
sbas_t *sbas;
typedef GLONASS_SpaceNode<FloatT> glonass_t;
glonass_t *glonass;

typedef typename gps_t::Satellite::Ephemeris gps_ephemeris_t;
struct gps_ephemeris_raw_t : public gps_ephemeris_t::raw_t {
Expand All @@ -85,14 +89,24 @@ struct GNSS_Data {

typedef typename gps_t::Ionospheric_UTC_Parameters gps_iono_utc_t;

Loader() : gps(NULL), sbas(NULL) {
typedef typename glonass_t::Satellite::Ephemeris_with_GPS_Time glonass_ephemeris_t;
struct glonass_ephemeris_raw_t : public glonass_ephemeris_t::raw_t {
unsigned int super_frame;
unsigned int has_string;
glonass_ephemeris_raw_t() : has_string(0) {}
} glonass_ephemeris[24];

Loader() : gps(NULL), sbas(NULL), glonass(NULL) {
for(unsigned int i(0); i < sizeof(gps_ephemeris) / sizeof(gps_ephemeris[0]); ++i){
gps_ephemeris[i].svid = i + 1;
}
for(unsigned int i(0); i < sizeof(qzss_ephemeris) / sizeof(qzss_ephemeris[0]); ++i){
qzss_ephemeris[i].svid = i + 193;
qzss_ephemeris[i].is_qzss = true;
}
for(unsigned int i(0); i < sizeof(glonass_ephemeris) / sizeof(glonass_ephemeris[0]); ++i){
glonass_ephemeris[i].svid = i + 1;
}
}

/**
Expand Down Expand Up @@ -195,11 +209,66 @@ struct GNSS_Data {
return true;
}

bool load(const glonass_ephemeris_t &eph){
if(!glonass){return false;}
glonass->satellite(eph.svid).register_ephemeris(eph);
return true;
}

bool load_glonass(const GNSS_Data &data){
if(data.subframe.bytes != 16){return false;}
if((data.subframe.sv_number == 0)
|| (data.subframe.sv_number > (sizeof(glonass_ephemeris) / sizeof(glonass_ephemeris[0])))){
return false; // exclude unknown satellite, whose sv_number may be 255.
}
typename observer_t::u32_t *buf((typename observer_t::u32_t *)data.subframe.buffer);
typedef typename glonass_t::template BroadcastedMessage<typename observer_t::u32_t> parser_t;
unsigned int super_frame(buf[3] >> 16), frame(buf[3] & 0xF), string_no(parser_t::m(buf));

if((string_no >= 1) && (string_no <= 5)){ // immediate info. (ephemeris) with time property (in non-immediate info,)
glonass_ephemeris_raw_t &eph(glonass_ephemeris[data.subframe.sv_number - 1]);
if((eph.has_string > 0) && (eph.super_frame != super_frame)){
eph.has_string = 0; // clean up
}
eph.super_frame = super_frame;
eph.has_string |= (0x1 << (string_no - 1));
switch(string_no){ // string num
case 1: eph.template update_string1<0, 0>(buf); break;
case 2: eph.template update_string2<0, 0>(buf); break;
case 3: eph.template update_string3<0, 0>(buf); break;
case 4: eph.template update_string4<0, 0>(buf); break;
case 5: {
eph.template update_string5<0, 0>(buf);
if(frame == 4){
// TODO: require special care for 50th frame? @see Table 4.9 note (4)
}
break;
}
}
if(eph.has_string == 0x1F){ // get all ephemeris and time info. in the same super frame
// Ephemeris_with_Time::raw_t =(cast)=> Ephemeris_with_Time => Ephemeris_with_GPS_Time
glonass_ephemeris_t eph_converted(eph);
eph_converted.freq_ch = data.subframe.glonass_freq_ch; // frequency channel
eph_converted.t_b_gps += gps->is_valid_utc() // leap second correction
? gps->iono_utc().delta_t_LS
: gps_time_t::guess_leap_seconds(eph_converted.t_b_gps);
load(eph_converted); // register ephemeris
eph.has_string = 0;
}
return true;
}else{ // non-immediate info. except for time info. (almanac)

}
return false;
}

bool load(const GNSS_Data &data){
switch(data.subframe.gnssID){
// TODO: other satellite systems
case observer_t::gnss_svid_t::SBAS:
return load_sbas(data);
case observer_t::gnss_svid_t::GLONASS:
return load_glonass(data);
case observer_t::gnss_svid_t::QZSS:
if(data.subframe.bytes != sizeof(data.subframe.buffer)){return false;}
case observer_t::gnss_svid_t::GPS:
Expand Down
Loading

0 comments on commit e2b2097

Please sign in to comment.