def main():
from server import pypilotServer
server = pypilotServer()
client = pypilotClient(server)
boatimu = BoatIMU(client)
quiet = '-q' in sys.argv
lastprint = 0
while True:
t0 = time.monotonic()
server.poll()
client.poll()
data = boatimu.read()
if data and not quiet:
if t0 - lastprint > .25:
printline('pitch', data['pitch'], 'roll', data['roll'],
'heading', data['heading'])
lastprint = t0
boatimu.poll()
while True:
dt = 1 / boatimu.rate.value - (time.monotonic() - t0)
if dt < 0:
break
if dt > 0:
time.sleep(dt)
def __init__(self, server):
self.client = pypilotClient(server)
self.multiprocessing = server.multiprocessing
self.pipe, self.pipe_out = NonBlockingPipe('nmea pipe', self.multiprocessing)
if self.multiprocessing:
self.process = multiprocessing.Process(target=self.nmea_process, daemon=True)
self.process.start()
else:
self.process = False
self.setup()
def __init__(self, server):
self.client = pypilotClient(server)
self.multiprocessing = server.multiprocessing
if self.multiprocessing:
self.pipe, pipe = NonBlockingPipe('imu pipe', self.multiprocessing)
self.process = multiprocessing.Process(target=self.process, args=(pipe,), daemon=True)
self.process.start()
return
self.process = False
self.setup()
def __init__(self, server):
if True:
# direct connection to send raw sensors to calibration process is more
# efficient than routing through server (save up to 2% cpu on rpi zero)
self.cal_pipe, self.cal_pipe_process = NonBlockingPipe('cal pipe', True, sendfailok=True)
else:
self.cal_pipe, self.cal_pipe_process = False, False # use client
self.client = pypilotClient(server)
self.process = multiprocessing.Process(target=CalibrationProcess, args=(self.cal_pipe_process, self.client), daemon=True)
self.process.start()
self.cal_ready = False
def __init__(self, sensors=False):
self.sensors = sensors
if not sensors: # only signalk process for testing
self.client = pypilotClient()
self.multiprocessing = False
else:
server = sensors.client.server
self.multiprocessing = server.multiprocessing
self.client = pypilotClient(server)
self.initialized = False
self.missingzeroconfwarned = False
self.signalk_access_url = False
self.last_access_request_time = 0
self.sensors_pipe, self.sensors_pipe_out = NonBlockingPipe('signalk pipe', self.multiprocessing)
if self.multiprocessing:
import multiprocessing
self.process = multiprocessing.Process(target=self.process, daemon=True)
self.process.start()
else:
self.process = False
def __init__(self, server):
if True:
# direct connection to send raw sensors to calibration process is more
# efficient than routing through server (save up to 2% cpu on rpi zero)
self.cal_pipe, cal_pipe = NonBlockingPipe('cal pipe',
True,
sendfailok=True)
else:
self.cal_pipe, cal_pipe = False, False # use client
client = pypilotClient(server)
super(AutomaticCalibrationProcess,
self).__init__(target=CalibrationProcess,
args=(cal_pipe, client),
daemon=True)
self.start()
def connect(self, dt):
if self.client:
return
watchlist = [
'ap.enabled', 'ap.mode', 'ap.heading', 'ap.heading_command'
]
def on_con(client):
for name in watchlist:
client.watch(name)
try:
self.client = pypilotClient(on_con, autoreconnect=True)
pass
except:
return
def main():
for i in range(len(sys.argv)):
if sys.argv[i] == '-t':
if len(sys.argv) < i + 2:
print(_('device needed for option') + ' -t')
exit(1)
test(sys.argv[i + 1])
print('pypilot Servo')
from server import pypilotServer
server = pypilotServer()
from client import pypilotClient
client = pypilotClient(server)
from sensors import Sensors # for rudder feedback
sensors = Sensors(client, False)
servo = Servo(client, sensors)
period = .1
start = lastt = time.monotonic()
while True:
if servo.controller.value != 'none':
print('voltage:', servo.voltage.value, 'current',
servo.current.value, 'ctrl temp',
servo.controller_temp.value, 'motor temp',
servo.motor_temp.value, 'rudder pos',
sensors.rudder.angle.value, 'flags', servo.flags.get_str())
pass
servo.poll()
sensors.poll()
client.poll()
server.poll()
dt = period - time.monotonic() + lastt
if dt > 0 and dt < period:
time.sleep(dt)
lastt += period
else:
lastt = time.monotonic()
def __init__(self):
super(Autopilot, self).__init__()
self.watchdog_device = False
self.server = pypilotServer()
self.client = pypilotClient(self.server)
self.boatimu = BoatIMU(self.client)
self.sensors = Sensors(self.client)
self.servo = servo.Servo(self.client, self.sensors)
self.version = self.register(Value, 'version', 'pypilot' + ' ' + strversion)
self.timestamp = self.client.register(SensorValue('timestamp', 0))
self.starttime = time.monotonic()
self.mode = self.register(ModeProperty, 'mode')
self.preferred_mode = self.register(Value, 'preferred_mode', 'compass')
self.lastmode = False
self.mode.ap = self
self.heading_command = self.register(HeadingProperty, 'heading_command', self.mode)
self.enabled = self.register(BooleanProperty, 'enabled', False)
self.lastenabled = False
self.last_heading = False
self.last_heading_off = self.boatimu.heading_off.value
self.pilots = {}
for pilot_type in pilots.default:
try:
pilot = pilot_type(self)
self.pilots[pilot.name] = pilot
except Exception as e:
print('failed to load pilot', pilot_type, e)
pilot_names = list(self.pilots)
print('Loaded Pilots:', pilot_names)
self.pilot = self.register(EnumProperty, 'pilot', 'basic', pilot_names, persistent=True)
self.heading = self.register(SensorValue, 'heading', directional=True)
self.heading_error = self.register(SensorValue, 'heading_error')
self.heading_error_int = self.register(SensorValue, 'heading_error_int')
self.heading_error_int_time = time.monotonic()
self.tack = tacking.Tack(self)
self.gps_compass_offset = HeadingOffset()
self.gps_speed = 0
self.wind_compass_offset = HeadingOffset()
self.true_wind_compass_offset = HeadingOffset()
self.wind_direction = self.register(SensorValue, 'wind_direction', directional=True)
self.wind_speed = 0
self.runtime = self.register(TimeValue, 'runtime') #, persistent=True)
self.timings = self.register(SensorValue, 'timings', False)
device = '/dev/watchdog0'
try:
self.watchdog_device = open(device, 'w')
except:
print('warning: failed to open special file', device, 'for writing')
print(' cannot stroke the watchdog')
self.server.poll() # setup process before we switch main process to realtime
if os.system('sudo chrt -pf 1 %d 2>&1 > /dev/null' % os.getpid()):
print('warning, failed to make autopilot process realtime')
self.lasttime = time.monotonic()
# setup all processes to exit on any signal
self.childprocesses = [self.boatimu.imu, self.boatimu.auto_cal,
self.sensors.nmea, self.sensors.gpsd,
self.sensors.signalk, self.server]
def cleanup(signal_number, frame=None):
#print('got signal', signal_number, 'cleaning up')
if signal_number == signal.SIGCHLD:
pid = os.waitpid(-1, os.WNOHANG)
#print('sigchld waitpid', pid)
if signal_number != 'atexit': # don't get this signal again
signal.signal(signal_number, signal.SIG_IGN)
while self.childprocesses:
process = self.childprocesses.pop().process
if process:
pid = process.pid
#print('kill', pid, process)
try:
os.kill(pid, signal.SIGTERM) # get backtrace
except Exception as e:
pass
#print('kill failed', e)
sys.stdout.flush()
if signal_number != 'atexit':
raise KeyboardInterrupt # to get backtrace on all processes
# unfortunately we occasionally get this signal,
# some sort of timing issue where python doesn't realize the pipe
# is broken yet, so doesn't raise an exception
def printpipewarning(signal_number, frame):
print('got SIGPIPE, ignoring')
import signal
for s in range(1, 16):
if s == 13:
signal.signal(s, printpipewarning)
elif s != 9:
signal.signal(s, cleanup)
signal.signal(signal.SIGCHLD, cleanup)
import atexit
atexit.register(lambda : cleanup('atexit'))
def main():
for i in range(len(sys.argv)):
if sys.argv[i] == '-t':
if len(sys.argv) < i + 2:
print(_('device needed for option') + ' -t')
exit(1)
test(sys.argv[i + 1])
print('pypilot Servo')
from server import pypilotServer
server = pypilotServer()
from client import pypilotClient
client = pypilotClient(server)
from sensors import Sensors # for rudder feedback
sensors = Sensors(client)
servo = Servo(client, sensors)
print('initializing arduino')
config = {
'host': 'localhost',
'hat': {
'arduino': {
'device': '/dev/spidev0.1',
'resetpin': '16'
}
},
'arduino.nmea.baud': 4800,
'arduino.nmea.in': False,
'arduino.nmea.out': False,
'arduino.ir': True,
'arduino.debug': True
}
a = arduino(config)
dt = 0
period = 0.0
start = lastt = time.monotonic()
while True:
events = a.poll()
if events:
print(events)
for key, count in events:
if key != 'voltage':
#print('go', time.monotonic())
if count:
servo.command.set(1)
else:
servo.command.set(0)
servo.poll()
sensors.poll()
client.poll()
server.poll()
dt = period - time.monotonic() + lastt
if dt > 0 and dt < period:
time.sleep(dt)
lastt += period
else:
lastt = time.monotonic()
if not socket.socket:
print('server socket closed from flush!!')
self.RemoveSocket(socket)
break
else:
break
for pipe in self.pipes:
pipe.flush()
if __name__ == '__main__':
server = pypilotServer()
from client import pypilotClient
from values import *
client1 = pypilotClient(server) # direct pipe to server
clock = client1.register(Value('clock', 0))
test1 = client1.register(Property('test', 1234))
print('client values1', client1.values)
client1.watch('test2', 10)
client2 = pypilotClient('localhost') # tcp socket connection
test2 = client2.register(Property('test2', [1, 2, 3, 4]))
client2.watch('clock', 1)
client3 = pypilotClient('localhost')
client3.watch('clock', 3)
def print_msgs(name, msgs):
for msg in msgs:
print(name, msg, msgs[msg])
