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 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()