from math import atan2, degrees, sin, cos, acos, radians, pi, sqrt
from machine import Pin, I2C, UART, SPI
from time import ticks_ms, sleep
from pico_i2c_lcd import I2cLcd
from qmc5883l import QMC5883L
import os
import gps_decode
import sdcard
#--- Datatypes --------------------------------------------------------------------
class Button:
down = False
long_down = False
hold = False
time = 0
class Buttons:
left = Button()
right = Button()
sw_set = False
sw_set_old = False
sw_mode = False
class Gps:
latitude = 0.0
longitude = 0.0
#--- Constants --------------------------------------------------------------------
GPS_BAUDRATE = 9600 # UART baudrate
BUTTON_LEFT = 6 # Left button pin
BUTTON_RIGHT = 7 # Right button pin
SWITCH_SET = 8 # Switch select pin
SWITCH_MODE = 9 # Switch mode pin
I2C_SDA = 4 # SDA pin
I2C_SCL = 5 # SCL pin
DISPLAY_ADDR = 0x27 # Display address
INDICATOR_RES = 6 # Angle indicator
INDICATOR_BAR = 7 # Number of segments per side (left/right)
BUTTON_DELAY = 50 # Short press delay buttons (ms)
BUTTON_LONG_DELAY = 1000 # Long press delay buttons (ms)
FILE_GPS = "gps.txt"
DIR_GPS = "sd/" # lomitko (/) na konci
FILE_CALIB = "GPS_Navig/compass.cal"
SAMPLE_NR = 3 # Number of GPS samples
CALIB_TIME = 10000 # Compass calibration time (ms)
#--- Compass default calibration data -----------------------
COMPASS_MIN_X = 0.1235
COMPASS_MAX_X = 0.61475
COMPASS_MIN_Y = -0.20375
COMPASS_MAX_Y = 0.3280833
#------------------------------------------------------------------------------------
error = 0
fatal_error = 0
#--- UART init --------------------------------------------------------------------
uart = UART(0, baudrate=GPS_BAUDRATE)
uart.init(GPS_BAUDRATE, bits=8, parity=None, stop=1)
#--- SPI init ---------------------------------------------------------------------
spi=SPI(0,baudrate=40000000,sck=Pin(18),mosi=Pin(19),miso=Pin(16))
try:
sd=sdcard.SDCard(spi,Pin(17))
os.mount(sd,'/sd')
except:
error = 1
#--- I2C init ---------------------------------------------------------------------
i2c = I2C(0, sda = Pin(I2C_SDA), scl = Pin(I2C_SCL), freq=400000)
#--- Compass init -----------------------------------------------------------------
try:
qmc = QMC5883L(i2c)
except:
fatal_error = 1
#--- Display init -----------------------------------------------------------------
lcd = I2cLcd(i2c, DISPLAY_ADDR, 2, 16)
lcd.backlight_on()
lcd.custom_char(0, bytearray([4, 14, 31, 31, 31, 31, 27, 17])) # arrow up
lcd.custom_char(1, bytearray([0, 0, 30, 24, 20, 18, 1, 0])) # arrow left up
lcd.custom_char(2, bytearray([0, 0, 15, 3, 5, 9 , 16, 0])) # arrow right up
lcd.custom_char(3, bytearray([0, 4, 8, 31, 8, 4, 0, 0])) # arrow left
lcd.custom_char(4, bytearray([0, 4, 2, 31, 2, 4, 0, 0])) # arrow right
#--- Buttons init -----------------------------------------------------------------
button_left = Pin(BUTTON_LEFT, Pin.IN, Pin.PULL_UP)
button_right = Pin(BUTTON_RIGHT, Pin.IN, Pin.PULL_UP)
switch_set = Pin(SWITCH_SET, Pin.IN, Pin.PULL_UP)
switch_mode = Pin(SWITCH_MODE, Pin.IN, Pin.PULL_UP)
buttons = Buttons()
buttons.sw_set_old = buttons.sw_set
#--- GPS data type init -----------------------------------------------------------
gps_actual = gps_decode.Gps()
gps_set = Gps()
gps_home_stanovice = [50.399290, 15.871351]
gps_home_stikov = [50.483709, 15.530718]
waypoint_set = 0
waypoints = []
#--- Variables --------------------------------------------------------------------
receive_line = ''
write_on = False
gps_data_indicator = False
receive_indicator = True
azimuth = 0
distance = 0
compass = 0
save_mode = False
first_save = True
actual_mode = 0 # 0 = RunLoop, 1 = InputLoop, 2 = noSigLoop
#------------------------------------------------------------------------------------
#******************************************************************************************************************************
def fileRead():
global waypoints, waypoint_nr, error
try:
file_data = open(DIR_GPS + FILE_GPS, "r")
latitude = 0.0
longitude = 0.0
text = file_data.read()
text = text.replace('\r', ',')
text = text.split(',')
for n in range(0, len(text), 2):
try:
latitude = float(text[n])
longitude = float(text[n + 1])
waypoints.append([latitude, longitude])
except:
break
file_data.close()
except:
if not error: error = 2
#******************************************************************************************************************************
def file_backup():
exists = True if FILE_GPS in os.listdir(DIR_GPS) else False
if exists:
name = FILE_GPS.split('.')
n = 0
list_dir = os.listdir(DIR_GPS)
while ((name[0] + str(n) + '.' + name[1]) in list_dir):
n += 1
file_to_write = name[0] + str(n) + '.' + name[1]
fr = open(DIR_GPS + FILE_GPS, "r")
fw = open(DIR_GPS + file_to_write, "w")
fw.write(fr.read())
fr.close()
fw.close()
#******************************************************************************************************************************
def file_save():
file = open(DIR_GPS + FILE_GPS, "w")
for n in range(0, len(waypoints)):
file.write(str(waypoints[n][0]) + ", " + str(waypoints[n][1]) + chr(13))
file.close()
#******************************************************************************************************************************
def read_compass(sensor):
mag_x, mag_y, _ = sensor.magnetic
mag_x = mag_x - ((COMPASS_MAX_X - COMPASS_MIN_X) / 2 + COMPASS_MIN_X)
mag_y = mag_y - ((COMPASS_MAX_Y - COMPASS_MIN_Y) / 2 + COMPASS_MIN_Y)
angle = degrees(atan2(mag_y, mag_x))
if angle < 0:
angle = angle + 360
return int(angle)
#******************************************************************************************************************************
def compass_calib(sensor):
global COMPASS_MIN_X, COMPASS_MAX_X, COMPASS_MIN_Y, COMPASS_MAX_Y
min_x = COMPASS_MIN_X
max_x = COMPASS_MAX_X
min_y = COMPASS_MIN_Y
max_y = COMPASS_MAX_Y
COMPASS_MIN_X = 30000
COMPASS_MAX_X = -30000
COMPASS_MIN_Y = 30000
COMPASS_MAX_Y = -30000
move = True
time = ticks_ms() + CALIB_TIME
lcd.clear()
lcd_print(" Kalibrace ", 0, 0)
while (time < move_time):
time = ticks_ms()
mag_x, mag_y, _ = sensor.magnetic
if mag_x < COMPASS_MIN_X:
COMPASS_MIN_X = mag_x
move = True
if mag_x > COMPASS_MAX_X:
COMPASS_MAX_X = mag_x
move = True
if mag_y < COMPASS_MIN_Y:
COMPASS_MIN_Y = mag_y
move = True
if mag_y > COMPASS_MAX_Y:
COMPASS_MAX_Y = mag_y
move = True
if move:
time = time + CALIB_TIME
move = False
lcd_print(zero_space((move_time - time) / 1000, 2), 0, 1)
if COMPASS_MIN_X < COMPASS_MAX_X and COMPASS_MIN_Y < COMPASS_MAX_Y:
lcd_print(" Kalibrace OK " , 0, 0)
lcd_print("Ne [Ulozit] Ano", 0, 1)
while (True):
keyDectect()
if buttons.right.long_down:
buttons.right.long_down = False
break
if buttons.left.down:
buttons_left.down = False
COMPASS_MIN_X = min_x
COMPASS_MAX_X = max_x
COMPASS_MIN_Y = min_y
COMPASS_MAX_Y = max_y
break
else:
lcd_print("Chyba kalibrace " , 0, 0)
COMPASS_MIN_X = min_x
COMPASS_MAX_X = max_x
COMPASS_MIN_Y = min_y
COMPASS_MAX_Y = max_y
sleep(5)
#****************************************************************************************************************************
def load_compass_calib_data():
global COMPASS_MIN_X, COMPASS_MAX_X, COMPASS_MIN_Y, COMPASS_MAX_Y, error
min_x = COMPASS_MIN_X
max_x = COMPASS_MAX_X
min_y = COMPASS_MIN_Y
max_y = COMPASS_MAX_Y
try:
file_data = open(FILE_CALIB, "r")
text = file_data.read()
text = text.split(',')
COMPASS_MIN_X = float(text[0])
COMPASS_MAX_X = float(text[1])
COMPASS_MIN_Y = float(text[2])
COMPASS_MAX_Y = float(text[3])
file_data.close()
except:
COMPASS_MIN_X = min_x
COMPASS_MAX_X = max_x
COMPASS_MIN_Y = min_y
COMPASS_MAX_Y = max_y
if not error: error = 3
#******************************************************************************************************************************
def read_gps():
global uart, receive_line, write_on
while uart.any():
try:
receive_byte = uart.read(1)
receive_byte = receive_byte.decode("utf-8")
except:
print(receive_line)
break;
if write_on:
if receive_byte != "\n":
receive_line += receive_byte;
else:
read_line(receive_line)
# print(receive_line)
write_on = False
else:
if receive_byte == "$":
receive_line = receive_byte
write_on = True
#******************************************************************************************************************************
def read_line(line):
global gps_actual, gps_data_indicator
# print(line)
try:
line = line.split(",")
except:
print('Error: read_line')
if line[0].startswith('$GPRMC') and len(line) == 13:
gps_actual.write_time(line[1])
gps_actual.write_status(line[2])
gps_actual.write_date(line[9])
gps_actual.write_position(line[3], line[5])
gpsDataCalc()
gps_data_indicator = True
#******************************************************************************************************************************
def zero_space(value : int, number_of_digit : int, decimal = 0, zero_visible = False):
string = (f'%0{number_of_digit}d' % value) if zero_visible else (f'% {number_of_digit}d' % value)
if decimal: string += (f'%.{decimal}f' % (value - int(value)))[1:]
return string
#******************************************************************************************************************************
def lcd_print(string: str, x: int, y: int):
lcd.move_to(x, y)
lcd.putstr(string)
#******************************************************************************************************************************
def gpsDataCalc():
global distance, azimuth
act_lat = radians(gps_actual.latitude)
act_lng = radians(gps_actual.longitude)
set_lat = radians(gps_set.latitude)
set_lng = radians(gps_set.longitude)
r = 6371 # Polomer zemekoule
lat_diff = (act_lat - set_lat) / 2
lng_diff = (act_lng - set_lng) / 2
compute = sin(lat_diff) * sin(lat_diff) + sin(lng_diff) * sin(lng_diff) * cos(act_lat) * cos(set_lat)
distance = r * (2 * atan2(sqrt(compute), sqrt(1 - compute)) * 1000) # Vzdalenost v metrech
if distance > 99999: distance = 99999 # Omezeni kvuli zobrazeni
course = atan2(sin(set_lng - act_lng) * cos(set_lat), (cos(act_lat) * sin(set_lat)) - (sin(act_lat) * cos(set_lat) * cos(set_lng - act_lng)))
if course < 0:
course += 2 * pi
course = degrees(course)
course += 0.5 # Zaokrouhleni
if course >= 360:
course -= 360
azimuth = int(course)
#******************************************************************************************************************************
def keyDetect():
global buttons
# button_left_state = False if button_left.value() else True
# button_right_state = False if button_right.value() else True
keyStatus(buttons.left, False if button_left.value() else True)
keyStatus(buttons.right, False if button_right.value() else True)
buttons.sw_set = False if switch_set.value() else True
buttons.sw_mode = False if switch_mode.value() else True
# time = ticks_ms()
#--- Left button -----------------------------------
# if time > (buttons.left.time + BUTTON_DELAY):
# if buttons.left.hold and not button_left_state: # off
# buttons.left.hold = False
# buttons.left.down = False
# buttons.left.long_down = False
# buttons.left.time = ticks_ms()
# if button_left_state and not buttons.left.hold: # short press
# buttons.left.down = True
# buttons.left.hold = True
# buttons.left.time = ticks_ms()
# if buttons.left.down and not buttons.left.long_down and time > (buttons.left.time + BUTTON_LONG_DELAY): # long press
# buttons.left.down = False
# buttons.left.long_down = True
#--- Right button ----------------------------------
# if time > (buttons.right.time + BUTTON_DELAY):
# if buttons.right.hold and not button_right_state: # off
# buttons.right.hold = False
# buttons.right.down = False
# buttons.right.long_down = False
# buttons.right.time = ticks_ms()
# if button_right_state and not buttons.right.hold: # short press
# buttons.right.down = True
# buttons.right.hold = True
# buttons.right.time = ticks_ms()
# if buttons.right.down and not buttons.right.long_down and time > (buttons.right.time + BUTTON_LONG_DELAY): # long press
# buttons.right.down = False
# buttons.right.long_down = True
#******************************************************************************************************************************
def keyStatus(key : Button, key_state : bool):
global buttons
time = ticks_ms()
if time > (key.time + BUTTON_DELAY):
if key.hold and not key_state: # off
key.hold = False
key.down = False
key.long_down = False
key.time = ticks_ms()
if key_state and not key.hold: # short press
key.down = True
key.hold = True
key.time = ticks_ms()
if key.down and not key.long_down and time > (key.time + BUTTON_LONG_DELAY): # long press
key.down = False
key.long_down = True
#******************************************************************************************************************************
def displayRun():
line = ''
azim = compass - azimuth
if azim < -179: azim += 360
if azim > 179: azim -= 360
dir = int(azim / INDICATOR_RES)
if dir < -7: dir = -7
if dir > 7: dir = 7
for i in range(-7, 8):
if i == dir:
if dir == 0:
line += chr(0) + chr(0) # arrow up
if dir < 0:
if dir <= -INDICATOR_BAR:
line += chr(4) # arrow right
else:
line += chr(2) # arrow right up
if dir > 0:
if dir >= INDICATOR_BAR:
line += chr(3) # arrow left
else:
line += chr(1) # arrow left up
else:
if i == 0: line += ' '
line += ' '
lcd_print(str(line), 0, 0)
lcd_print("Vzdalenost%5dm" % distance, 0, 1)
#******************************************************************************************************************************
def waypointSelect():
global waypoint_set
keyDetect()
if buttons.left.down and waypoint_set > -2:
buttons.left.down = False
waypoint_set -= 1
if buttons.right.down and waypoint_set < (len(waypoints) - 1):
buttons.right.down = False
waypoint_set += 1
lcd_print(" Vyber bod ", 0, 0)
if waypoint_set == -1:
lcd_print(" Domu Stanovice ", 0, 1)
elif waypoint_set == -2:
lcd_print(" Domu Stikov ", 0, 1)
else:
lcd_print(" " + zero_space(waypoint_set, 2) + " ", 0, 1)
#******************************************************************************************************************************
def add_waypoint():
global waypoints, waypoint_set, gps_data_indicator, millis
latitude = 0
longitude = 0
n = 0
while (n < SAMPLE_NR):
# read_gps()
if ticks_ms() > millis:
read_line("$GPRMC,152452.00,A,5024.00150,N,01552.27440,E,1.593,258.09,070223,,,A*6D")
millis = ticks_ms() + 1000
if gps_data_indicator:
latitude += gps_actual.latitude
longitude += gps_actual.longitude
n += 1
gps_data_indicator = False
latitude /= SAMPLE_NR
longitude /= SAMPLE_NR
waypoints.append([latitude, longitude])
waypoint_set += 1
#******************************************************************************************************************************
def runLoop():
global buttons, actual_mode, compass
if actual_mode != 0:
lcd.clear()
actual_mode = 0
keyDetect()
if buttons.sw_set: # Run mode
if not buttons.sw_set_old:
if waypoint_set >= 0:
gps_set.latitude = waypoints[waypoint_set][0]
gps_set.longitude = waypoints[waypoint_set][1]
else:
if waypoint_set == -1:
gps_set.latitude = gps_home_stanovice[0]
gps_set.longitude = gps_home_stanovice[1]
else:
gps_set.latitude = gps_home_stikov[0]
gps_set.longitude = gps_home_stikov[1]
gpsDataCalc()
else:
compass = read_compass(qmc)
displayRun()
else: # Set mode
waypointSelect()
buttons.sw_set_old = buttons.sw_set
#******************************************************************************************************************************
def inputLoop():
global save_mode, buttons, actual_mode, waypoints, waypoint_set, first_save
if actual_mode != 1:
lcd.clear()
actual_mode = 1
keyDetect()
if save_mode:
if buttons.right.long_down:
buttons.right.long_down = False
if first_save:
file_backup()
waypoints = []
waypoint_set = 0
first_save = False
lcd.clear()
lcd_print(" Ukladam ", 0, 0)
add_waypoint()
file_save()
lcd.clear()
lcd_print(zero_space(str(waypoint_set - 1), 3), 12, 0)
lcd_print("OK", 13, 1)
save_mode = False
if buttons.left.down:
buttons.left.down = False
save_mode = False;
lcd.clear()
else:
if gps_actual.latitude:
lcd_print('%.5f' % gps_actual.latitude, 0, 0)
else:
lcd_print("--.------", 0, 0)
if gps_actual.longitude:
lcd_print('%.5f' % gps_actual.longitude, 0, 1)
else:
lcd_print("--.------", 0, 1)
if buttons.left.long_down:
buttons.left.long_down = False
save_mode = True
lcd_print("Ulozit bod " + zero_space(str(waypoint_set), 3) + " ?", 0, 0)
lcd_print(" Ne Ano ", 0, 1)
#******************************************************************************************************************************
def noSigLoop():
global actual_mode
if actual_mode != 2:
lcd.clear()
actual_mode = 2
lcd_print("Cekam na signal", 0, 0)
if receive_indicator:
lcd_print('*', 15, 0)
else:
lcd_print(' ', 15, 0)
if gps_actual.time:
lcd_print(zero_space(gps_actual.time[0], 2, 0, True) + ":" + zero_space(gps_actual.time[1], 2, 0, True) + ":" + zero_space(gps_actual.time[2], 2, 0, True), 0, 1)
else:
lcd_print("--:--:--", 0, 1)
#******************************************************************************************************************************
#*** Setup ******************************************************************************************************************
load_compass_calib_data()
fileRead()
if fatal_error:
if fatal_error == 1: lcd_print(" Chyba kompasu ", 0, 0)
# while(True): pass
sleep(10)
if error:
lcd.clear()
if error == 1: lcd_print(" Chybi SD karta ", 0, 0)
if error == 2: lcd_print("Karta je prazdna", 0, 0)
if error == 3: # Compass
lcd_print(" Nelze nacist ", 0, 0)
lcd_print("kalibracni data ", 0, 1)
sleep(5)
if len(waypoints) > 0:
waypoint_set = 0
else:
waypoint_set = -1
#*** Loop *******************************************************************************************************************
millis = ticks_ms()
while True:
# read_gps()
if ticks_ms() > millis:
read_line("$GPRMC,152452.00,A,5024.00150,N,01552.27440,E,1.593,258.09,070223,,,A*6D")
millis = ticks_ms() + 1000
# keyDetect()
if gps_actual.status:
if buttons.sw_mode:
inputLoop()
else:
runLoop()
else:
noSigLoop()
if gps_data_indicator:
receive_indicator = False if receive_indicator else True
gps_data_indicator = False
# print(f"heading: {get_heading(qmc)} degrees")#, COMPASS_MIN_X: {COMPASS_MIN_X}, max_x: {COMPASS_MAX_X}, min_y: {COMPASS_MIN_Y}, max_y: {COMPASS_MAX_Y}")
#******************************************************************************************************************************