79 lines
2.1 KiB
Python
79 lines
2.1 KiB
Python
|
import time
|
||
|
|
import board
|
||
|
|
import busio
|
||
|
|
import adafruit_bno055
|
||
|
|
import adafruit_gps
|
||
|
|
import digitalio
|
||
|
|
from car_os.network_handler import NetworkHandler
|
||
|
|
import math
|
||
|
|
from car_os.pps import PPS
|
||
|
|
from car_os.gps import GPS
|
||
|
|
from car_os.imu import IMU
|
||
|
|
from car_os.timer import Timer
|
||
|
|
from car_os.enums import SensorClass
|
||
|
|
|
||
|
|
def main():
|
||
|
|
heartbeat_timer = Timer(15)
|
||
|
|
|
||
|
|
network = NetworkHandler(SensorClass.VEHICLE, 256, 0.001, 5000, 0.1, 5001)
|
||
|
|
network.initialize()
|
||
|
|
network_timer = Timer(0.5)
|
||
|
|
|
||
|
|
pps = PPS(board.GP22)
|
||
|
|
imu = IMU(board.GP21, board.GP20, 0x0C, (44, 66, -109), (-1, 3, -1), (-7, -16, -39))
|
||
|
|
gps = GPS(board.GP27, board.GP26, 0x42)
|
||
|
|
|
||
|
|
while True:
|
||
|
|
pps.update()
|
||
|
|
gps.update(pps.utc)
|
||
|
|
imu.update(pps.utc)
|
||
|
|
|
||
|
|
if not gps.has_fix:
|
||
|
|
print(time.time())
|
||
|
|
print("waiting for gps fix")
|
||
|
|
print()
|
||
|
|
continue
|
||
|
|
|
||
|
|
if gps.time_updated:
|
||
|
|
pps.update_from_gps(gps)
|
||
|
|
|
||
|
|
if network_timer.check_timer(pps.utc):
|
||
|
|
network.update(pps.utc, network_timer)
|
||
|
|
|
||
|
|
#if heartbeat_timer.check_timer(pps.utc):
|
||
|
|
# message = bytearray(b'\xd4\x53\x6e\x4e\x00\x00\x00')
|
||
|
|
# message[4] = pps.utc[0]
|
||
|
|
# message[5] = pps.utc[1]
|
||
|
|
# message[6] = pps.utc[2]
|
||
|
|
# message += b'\x00\x00'
|
||
|
|
# network.send(message)
|
||
|
|
|
||
|
|
if not gps.ready:
|
||
|
|
continue
|
||
|
|
try:
|
||
|
|
message = gps.get_position_message()
|
||
|
|
if message is not None:
|
||
|
|
network.send(message)
|
||
|
|
except:
|
||
|
|
pass
|
||
|
|
try:
|
||
|
|
message = gps.get_sat_message()
|
||
|
|
if message is not None:
|
||
|
|
network.send(message)
|
||
|
|
except:
|
||
|
|
pass
|
||
|
|
try:
|
||
|
|
message = imu.get_accel_message()
|
||
|
|
if message is not None:
|
||
|
|
network.send(message)
|
||
|
|
except:
|
||
|
|
pass
|
||
|
|
try:
|
||
|
|
message = imu.get_mag_message()
|
||
|
|
if message is not None:
|
||
|
|
network.send(message)
|
||
|
|
except:
|
||
|
|
pass
|
||
|
|
|
||
|
|
main()
|