@ -4,8 +4,11 @@ from typing import List
import numpy as np import numpy as np
from collections import defaultdict from collections import defaultdict
from numpy . linalg import linalg
from cereal import log , messaging from cereal import log , messaging
from laika import AstroDog from laika import AstroDog
from laika . ephemeris import convert_ublox_ephem
from laika . helpers import ConstellationId from laika . helpers import ConstellationId
from laika . raw_gnss import GNSSMeasurement , calc_pos_fix , correct_measurements , process_measurements , read_raw_ublox from laika . raw_gnss import GNSSMeasurement , calc_pos_fix , correct_measurements , process_measurements , read_raw_ublox
from selfdrive . locationd . models . constants import GENERATED_DIR , ObservationKind from selfdrive . locationd . models . constants import GENERATED_DIR , ObservationKind
@ -19,28 +22,25 @@ MAX_TIME_GAP = 10
class Laikad : class Laikad :
def __init__ ( self ) : def __init__ ( self , use_internet ) :
self . astro_dog = AstroDog ( use_internet = use_internet )
self . gnss_kf = GNSSKalman ( GENERATED_DIR ) self . gnss_kf = GNSSKalman ( GENERATED_DIR )
def process_ublox_msg ( self , ublox_msg , dog : AstroDog , ublox_mono_time : int ) : def process_ublox_msg ( self , ublox_msg , ublox_mono_time : int ) :
if ublox_msg . which == ' measurementReport ' : if ublox_msg . which == ' measurementReport ' :
report = ublox_msg . measurementReport report = ublox_msg . measurementReport
new_meas = read_raw_ublox ( report ) new_meas = read_raw_ublox ( report )
measurements = process_measurements ( new_meas , dog ) measurements = process_measurements ( new_meas , self . astro_dog )
pos_fix = calc_pos_fix ( measurements , min_measurements = 4 )
pos_fix = calc_pos_fix ( measurements )
# To get a position fix a minimum of 5 measurements are needed. # To get a position fix a minimum of 5 measurements are needed.
# Each report can contain less and some measurement can't be processed. # Each report can contain less and some measurements can't be processed.
if len ( pos_fix ) > 0 : corrected_measurements = [ ]
measurements = correct_measurements ( measurements , pos_fix [ 0 ] [ : 3 ] , dog ) if len ( pos_fix ) > 0 and linalg . norm ( pos_fix [ 1 ] ) < 100 :
meas_msgs = [ create_measurement_msg ( m ) for m in measurements ] corrected_measurements = correct_measurements ( measurements , pos_fix [ 0 ] [ : 3 ] , self . astro_dog )
t = ublox_mono_time * 1e-9 t = ublox_mono_time * 1e-9
self . update_localizer ( pos_fix , t , corrected_measurements )
self . update_localizer ( pos_fix , t , measurements )
localizer_valid = self . localizer_valid ( t ) localizer_valid = self . localizer_valid ( t )
ecef_pos = self . gnss_kf . x [ GStates . ECEF_POS ] . tolist ( ) ecef_pos = self . gnss_kf . x [ GStates . ECEF_POS ] . tolist ( )
ecef_vel = self . gnss_kf . x [ GStates . ECEF_VELOCITY ] . tolist ( ) ecef_vel = self . gnss_kf . x [ GStates . ECEF_VELOCITY ] . tolist ( )
@ -49,6 +49,8 @@ class Laikad:
bearing_deg , bearing_std = get_bearing_from_gnss ( ecef_pos , ecef_vel , vel_std ) bearing_deg , bearing_std = get_bearing_from_gnss ( ecef_pos , ecef_vel , vel_std )
meas_msgs = [ create_measurement_msg ( m ) for m in corrected_measurements ]
dat = messaging . new_message ( " gnssMeasurements " ) dat = messaging . new_message ( " gnssMeasurements " )
measurement_msg = log . GnssMeasurements . Measurement . new_message measurement_msg = log . GnssMeasurements . Measurement . new_message
dat . gnssMeasurements = { dat . gnssMeasurements = {
@ -59,6 +61,11 @@ class Laikad:
" correctedMeasurements " : meas_msgs " correctedMeasurements " : meas_msgs
} }
return dat return dat
elif ublox_msg . which == ' ephemeris ' :
ephem = convert_ublox_ephem ( ublox_msg . ephemeris )
self . astro_dog . add_ephem ( ephem , self . astro_dog . orbits )
# elif ublox_msg.which == 'ionoData':
# todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them.
def update_localizer ( self , pos_fix , t : float , measurements : List [ GNSSMeasurement ] ) : def update_localizer ( self , pos_fix , t : float , measurements : List [ GNSSMeasurement ] ) :
# Check time and outputs are valid # Check time and outputs are valid
@ -67,9 +74,10 @@ class Laikad:
if len ( pos_fix ) == 0 : if len ( pos_fix ) == 0 :
return return
post_est = pos_fix [ 0 ] [ : 3 ] . tolist ( ) post_est = pos_fix [ 0 ] [ : 3 ] . tolist ( )
if self . gnss_kf . filter . filter_time is None : filter_time = self . gnss_kf . filter . filter_time
if filter_time is None :
cloudlog . info ( " Init gnss kalman filter " ) cloudlog . info ( " Init gnss kalman filter " )
elif ( self . gnss_kf . filter . filter_time - t ) > MAX_TIME_GAP : elif ( t - filter_time ) > MAX_TIME_GAP :
cloudlog . error ( " Time gap of over 10s detected, gnss kalman reset " ) cloudlog . error ( " Time gap of over 10s detected, gnss kalman reset " )
else : else :
cloudlog . error ( " Gnss kalman filter state is nan " ) cloudlog . error ( " Gnss kalman filter state is nan " )
@ -82,7 +90,7 @@ class Laikad:
def localizer_valid ( self , t : float ) : def localizer_valid ( self , t : float ) :
filter_time = self . gnss_kf . filter . filter_time filter_time = self . gnss_kf . filter . filter_time
return filter_time is not None and ( fil ter_time - t ) < MAX_TIME_GAP and \ return filter_time is not None and ( t - fil ter_time ) < MAX_TIME_GAP and \
all ( np . isfinite ( self . gnss_kf . x [ GStates . ECEF_POS ] ) ) all ( np . isfinite ( self . gnss_kf . x [ GStates . ECEF_POS ] ) )
def init_gnss_localizer ( self , est_pos ) : def init_gnss_localizer ( self , est_pos ) :
@ -97,11 +105,10 @@ def create_measurement_msg(meas: GNSSMeasurement):
c = log . GnssMeasurements . CorrectedMeasurement . new_message ( ) c = log . GnssMeasurements . CorrectedMeasurement . new_message ( )
c . constellationId = meas . constellation_id . value c . constellationId = meas . constellation_id . value
c . svId = meas . sv_id c . svId = meas . sv_id
observables = meas . observables_final
c . glonassFrequency = meas . glonass_freq if meas . constellation_id == ConstellationId . GLONASS else 0 c . glonassFrequency = meas . glonass_freq if meas . constellation_id == ConstellationId . GLONASS else 0
c . pseudorange = float ( observables [ ' C1C ' ] ) c . pseudorange = float ( meas . observables_final [ ' C1C ' ] )
c . pseudorangeStd = float ( meas . observables_std [ ' C1C ' ] ) c . pseudorangeStd = float ( meas . observables_std [ ' C1C ' ] )
c . pseudorangeRate = float ( observables [ ' D1C ' ] ) c . pseudorangeRate = float ( meas . observables_final [ ' D1C ' ] )
c . pseudorangeRateStd = float ( meas . observables_std [ ' D1C ' ] ) c . pseudorangeRateStd = float ( meas . observables_std [ ' D1C ' ] )
c . satPos = meas . sat_pos_final . tolist ( ) c . satPos = meas . sat_pos_final . tolist ( )
c . satVel = meas . sat_vel . tolist ( ) c . satVel = meas . sat_vel . tolist ( )
@ -134,19 +141,17 @@ def get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std):
def main ( ) : def main ( ) :
dog = AstroDog ( use_internet = True )
sm = messaging . SubMaster ( [ ' ubloxGnss ' ] ) sm = messaging . SubMaster ( [ ' ubloxGnss ' ] )
pm = messaging . PubMaster ( [ ' gnssMeasurements ' ] ) pm = messaging . PubMaster ( [ ' gnssMeasurements ' ] )
laikad = Laikad ( ) laikad = Laikad ( use_internet = True )
while True : while True :
sm . update ( ) sm . update ( )
# Todo if no internet available use latest ephemeris
if sm . updated [ ' ubloxGnss ' ] : if sm . updated [ ' ubloxGnss ' ] :
ublox_msg = sm [ ' ubloxGnss ' ] ublox_msg = sm [ ' ubloxGnss ' ]
msg = laikad . process_ublox_msg ( ublox_msg , dog , sm . logMonoTime [ ' ubloxGnss ' ] ) msg = laikad . process_ublox_msg ( ublox_msg , sm . logMonoTime [ ' ubloxGnss ' ] )
if msg is not None : if msg is not None :
pm . send ( ' gnssMeasurements ' , msg ) pm . send ( ' gnssMeasurements ' , msg )
Shane Smiskol
3 years ago