sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

better packet info

Browse Source 
old-commit-hash: 4470407729
commatwo_master
Harald Schafer 5 years ago
parent 6ee6d0a5eb
commit fd44a0f698
1 changed files with 11 additions and 6 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 17
      selfdrive/locationd/locationd.py

17
selfdrive/locationd/locationd.py
Unescape View File

@ -46,11 +46,16 @@ class Localizer():
fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])] fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])]
fix.accel = [float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21])] fix.accel = [float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21])]
local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY]) ned_vel = self.converter.ecef2ned(predicted_state[States.ECEF_POS] + predicted_state[States.ECEF_VELOCITY]) - self.converter.ecef2ned(predicted_state[States.ECEF_POS])
fix.pitchCalibration = math.degrees(math.atan2(local_vel[2], local_vel[0])) fix.vNED = [float(ned_vel[0]), float(ned_vel[1]), float(ned_vel[2])]
fix.yawCalibration = math.degrees(math.atan2(local_vel[1], local_vel[0])) fix.source = 'kalman'
#fix.imuFrame = [(180/np.pi)*float(predicted_state[23]), (180/np.pi)*float(predicted_state[24]), (180/np.pi)*float(predicted_state[25])] #local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY])
#fix.pitchCalibration = math.degrees(math.atan2(local_vel[2], local_vel[0]))
#fix.yawCalibration = math.degrees(math.atan2(local_vel[1], local_vel[0]))
imu_frame = predicted_state[States.IMU_OFFSET]
fix.imuFrame = [math.degrees(imu_frame[0]), math.degrees(imu_frame[1]), math.degrees(imu_frame[2])]
return fix return fix
def update_kalman(self, time, kind, meas): def update_kalman(self, time, kind, meas):
@ -67,8 +72,8 @@ class Localizer():
# self.observation_buffer.pop(0) # self.observation_buffer.pop(0)
def handle_gps(self, current_time, log): def handle_gps(self, current_time, log):
converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude]) self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
fix_ecef = converter.ned2ecef([0, 0, 0]) fix_ecef = self.converter.ned2ecef([0, 0, 0])
# TODO initing with bad bearing not allowed, maybe not bad? # TODO initing with bad bearing not allowed, maybe not bad?
if not self.filter_ready and log.speed > 5: if not self.filter_ready and log.speed > 5:

Write Preview
Loading…
Cancel
Save