34 lines
1.2 KiB
Python
34 lines
1.2 KiB
Python
|
from cereal import messaging
|
||
|
|
|
||
|
|
|
||
|
|
LOCATION1 = (32.7174, -117.16277)
|
||
|
|
LOCATION2 = (32.7558, -117.2037)
|
||
|
|
|
||
|
|
LLK_DECIMATION = 10
|
||
|
|
RENDER_FRAMES = 15
|
||
|
|
DEFAULT_ITERATIONS = RENDER_FRAMES * LLK_DECIMATION
|
||
|
|
|
||
|
|
|
||
|
|
def generate_liveLocationKalman(location=LOCATION1):
|
||
|
|
msg = messaging.new_message('liveLocationKalman')
|
||
|
|
msg.liveLocationKalman.positionGeodetic = {'value': [*location, 0], 'std': [0., 0., 0.], 'valid': True}
|
||
|
|
msg.liveLocationKalman.positionECEF = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
|
||
|
|
msg.liveLocationKalman.calibratedOrientationNED = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
|
||
|
|
msg.liveLocationKalman.velocityCalibrated = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
|
||
|
|
msg.liveLocationKalman.status = 'valid'
|
||
|
|
msg.liveLocationKalman.gpsOK = True
|
||
|
|
return msg
|
||
|
|
|
||
|
|
|
||
|
|
def generate_livePose():
|
||
|
|
msg = messaging.new_message('livePose')
|
||
|
|
meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True}
|
||
|
|
msg.livePose.orientationNED = meas
|
||
|
|
msg.livePose.velocityDevice = meas
|
||
|
|
msg.livePose.angularVelocityDevice = meas
|
||
|
|
msg.livePose.accelerationDevice = meas
|
||
|
|
msg.livePose.inputsOK = True
|
||
|
|
msg.livePose.posenetOK = True
|
||
|
|
msg.livePose.sensorsOK = True
|
||
|
|
return msg
|