def convert_and_send_as_nmea(out, data, gps_vel):
# https://opencpn.org/wiki/dokuwiki/doku.php?id=opencpn:opencpn_user_manual:advanced_features:nmea_sentences
# https://github.com/OpenCPN/OpenCPN/blob/c4fb6a0ad0205501ae902b57f9c64b7d0262a199/plugins/dashboard_pi/src/dashboard_pi.cpp
tws = -1.0
twd = -1.0
vmg = -1.0
try:
out.write(
pynmea2.MWV('II', 'MWV',
(str(float(data["m_wind.apparentAngle"])), "R",
data["m_wind.apparentSpeed"], "K", "A")).render(
True, True, True))
out.write(
pynmea2.HDM('II', 'HDM',
(data["yaw"], "M")).render(True, True, True))
out.write(
pynmea2.DPT('II', 'DPT', (data["m_depth.depthBelowTransductor"],
"0.5", "70.0")).render(True, True, True))
out.write(
pynmea2.MTW('II', 'MTW', (data["m_speed.waterTemp"], "C")).render(
True, True, True))
#out.write(pynmea2.VTG('II', 'VTG', ("0.0", "T", "0.0", "M", data["m_speed"], "N", str(float(data["m_speed"]) * 1.8), "K")).render(True, True, True))
out.write(
pynmea2.VHW('II', 'VHW',
("0.0", "T", "0.0", "M", data["m_speed"], "N",
str(float(data["m_speed"]) * 1.8), "K")).render(
True, True, True))
out.write(
pynmea2.XDR('II', 'XDR',
("A", str(-float(data["roll"])), "", "ROLL")).render(
True, True, True))
out.write(
pynmea2.XDR('II', 'XDR', ("A", data["pitch"], "", "PITCH")).render(
True, True, True))
angle = str(float(data["m_currentPosition"]) / 5)
out.write(
pynmea2.RSA('II', 'RSA',
(angle, "A", "0.0", "V")).render(True, True, True))
position_information = data["Position"].split("##")
if len(position_information) > 0:
for p in position_information:
out.write(p)
if DEBUG:
print("roll:" + data["roll"])
print("pitch:" + data["pitch"])
print("yaw:" + data["yaw"])
print("freq:" + data["freq"])
except Exception as e:
if DEBUG:
print("Exception during conversion1")
print(data)
print(e)
# https://opencpn.org/wiki/dokuwiki/doku.php?id=opencpn:developer_manual:plugins:beta_plugins:nmea_converter
try:
wind_direction = float(data["m_wind.apparentAngle"]) / 180.0 * math.pi
wind_speed = float(data["m_wind.apparentSpeed"])
boat_speed = float(data["m_speed"])
boat_speed = abs(float(gps_vel))
# print((boat_speed, wind_speed))
boat_direction = 0.0
u = boat_speed * math.sin(boat_direction) - wind_speed * math.sin(
wind_direction)
v = boat_speed * math.cos(boat_direction) - wind_speed * math.cos(
wind_direction)
tws = str(math.sqrt(u * u + v * v))
twd = 180.0 + math.atan2(u, v) * 180.0 / math.pi
if twd < 0.0:
twd += 360.0
vmg = str(boat_speed * math.cos(twd / 180.0 * math.pi))
twd = str(twd)
out.write(
pynmea2.MWV('II', 'MWV',
(twd, "T", tws, "N", "A")).render(True, True, True))
except (ValueError, ZeroDivisionError, KeyError) as e:
if DEBUG:
print(e)
return (tws, twd, vmg)
def work_pypilot():
#init compass
mode = conf.get('PYPILOT', 'mode')
if mode == 'disabled':
print 'pypilot disabled '
return
headingSK = conf.get('PYPILOT', 'translation_magnetic_h')
attitudeSK = conf.get('PYPILOT', 'translation_attitude')
SETTINGS_FILE = "RTIMULib"
s = RTIMU.Settings(SETTINGS_FILE)
imu = RTIMU.RTIMU(s)
imuName = imu.IMUName()
del imu
del s
if mode == 'imu':
cmd = ['pypilot_boatimu', '-q']
elif mode == 'basic autopilot':
# ensure no serial getty running
os.system('sudo systemctl stop [email protected]')
os.system('sudo systemctl stop [email protected]')
cmd = ['pypilot']
try:
translation_rate = float(conf.get('PYPILOT', 'translation_rate'))
except:
translation_rate = 1
conf.set('PYPILOT', 'translation_rate', '1')
pid = os.fork()
try:
if pid == 0:
os.execvp(cmd[0], cmd)
print 'failed to launch', cmd
exit(1)
except:
print 'exception launching pypilot'
exit(1)
print 'launched pypilot pid', pid
time.sleep(3) # wait 3 seconds to launch client
def on_con(client):
print 'connected'
if headingSK == '1':
client.watch('imu.heading')
if attitudeSK == '1':
client.watch('imu.pitch')
client.watch('imu.roll')
client = False
tick1 = time.time()
while read_sensors:
ret = os.waitpid(pid, os.WNOHANG)
if ret[0] == pid:
# should we respawn pypilot if it crashes?
print 'pypilot exited'
break
# connect to pypilot if not connected
try:
if not client:
client = SignalKClient(on_con, 'localhost')
except:
time.sleep(1)
continue # not much to do without connection
try:
result = client.receive()
except:
print 'disconnected from pypilot'
client = False
continue
Erg = Translate(result)
SignalK='{"updates":[{"$source":"OPsensors.I2C.'+imuName+'","values":['
SignalK+=Erg[0:-1]+'}]}]}\n'
sock.sendto(SignalK, ('127.0.0.1', 55557))
if mode == 'imu':
if 'imu.heading' in result:
value = result['imu.heading']['value']
hdm = str(pynmea2.HDM('AP', 'HDM', (str(value),'M')))+'\r\n'
sock.sendto(hdm, ('127.0.0.1', 10110))
if 'imu.roll' in result:
value = result['imu.roll']['value']
xdr_r = str(pynmea2.XDR('AP', 'XDR', ('A',str(value),'D','ROLL')))+'\r\n'
sock.sendto(xdr_r, ('127.0.0.1', 10110))
if 'imu.pitch' in result:
value = result['imu.pitch']['value']
xdr_p = str(pynmea2.XDR('AP', 'XDR', ('A',str(value),'D','PTCH')))+'\r\n'
sock.sendto(xdr_p, ('127.0.0.1', 10110))
while True:
dt = translation_rate - time.time() + tick1
if dt <= 0:
break
time.sleep(dt)
tick1 = time.time()
# cleanup
print 'stopping pypilot pid:', pid
try:
os.kill(pid, 15)
time.sleep(1) # wait one second to shut down pypilot
except Exception, e:
print 'exception stopping pypilot', e
list_tmp1.append('I2CH')
if nmea_heel_b and heel:
heel = round(heel, 1)
list_tmp2.append('A')
list_tmp2.append(str(heel))
list_tmp2.append('D')
list_tmp2.append('I2CX')
if nmea_pitch_b and pitch:
pitch = round(pitch, 1)
list_tmp2.append('A')
list_tmp2.append(str(pitch))
list_tmp2.append('D')
list_tmp2.append('I2CY')
if list_tmp1:
xdr = pynmea2.XDR('OS', 'XDR', (list_tmp1))
xdr1 = str(xdr)
xdr2 = xdr1 + "\r\n"
sock.sendto(xdr2, ('127.0.0.1', 10110))
if list_tmp2:
xdr = pynmea2.XDR('OS', 'XDR', (list_tmp2))
xdr1 = str(xdr)
xdr2 = xdr1 + "\r\n"
sock.sendto(xdr2, ('127.0.0.1', 10110))
heel = ''
pitch = ''
temperature = ''
if nmea_temp_p_b: temperature = temperature_p
if nmea_temp_h_b: temperature = temperature_h