class Sensors(object):
def __init__(self, server):
from gpsd import gpsd
from rudder import Rudder
from nmea import Nmea
self.server = server
self.nmea = Nmea(server, self)
self.gps = gpsd(server, self)
self.wind = Wind(server)
self.rudder = Rudder(server)
self.apb = APB(server)
self.sensors = {
'gps': self.gps,
'wind': self.wind,
'rudder': self.rudder,
'apb': self.apb
}
def poll(self):
self.gps.poll()
self.nmea.poll()
self.rudder.poll()
# timeout sources
t = time.time()
for name in self.sensors:
sensor = self.sensors[name]
if sensor.source.value == 'none':
continue
if t - sensor.lastupdate > 8:
self.lostsensor(sensor)
def lostsensor(self, sensor):
print('sensor', sensor.name, 'lost', sensor.source.value,
sensor.device)
sensor.source.set('none')
sensor.reset()
sensor.device = None
def write(self, sensor, data, source):
if not sensor in self.sensors:
print('unknown data parsed!', sensor)
return
self.sensors[sensor].write(data, source)
def lostdevice(self, device):
# optional routine useful when a device is
# unplugged to skip the normal data timeout
for name in self.sensors:
sensor = self.sensors[name]
if sensor.device and sensor.device[2:] == device:
self.lostsensor(sensor)
class Sensors(object):
def __init__(self, server):
from gpsd import Gpsd
from rudder import Rudder
from nmea import Nmea
self.server = server
self.nmea = Nmea(server, self)
self.gps = Gpsd(server, self)
self.wind = Wind(server)
self.rudder = Rudder(server)
self.sensors = {
'gps': self.gps,
'wind': self.wind,
'rudder': self.rudder
}
def poll(self):
self.gps.poll()
self.nmea.poll()
self.rudder.poll()
# timeout sources
t = time.time()
for name in self.sensors:
sensor = self.sensors[name]
if sensor.source.value == 'none':
continue
if t - sensor.lastupdate > 8:
print 'sensor timeout for', name, 'source', sensor.source.value, t - sensor.lastupdate
sensor.source.set('none')
sensor.device = None
def write(self, sensor, data, source):
if not sensor in self.sensors:
print 'unknown data parsed!', sensor
return
self.sensors[sensor].write(data, source)
class Sensors(object):
def __init__(self, client):
from rudder import Rudder
from nmea import Nmea
from signalk import signalk
self.client = client
# services that can receive sensor data
self.nmea = Nmea(self)
self.signalk = signalk(self)
self.gpsd = gpsd(self)
# actual sensors supported
self.gps = gps(client)
self.wind = Wind(client)
self.rudder = Rudder(client)
self.apb = APB(client)
self.sensors = {
'gps': self.gps,
'wind': self.wind,
'rudder': self.rudder,
'apb': self.apb
}
def poll(self):
self.nmea.poll()
self.signalk.poll()
self.gpsd.poll()
self.rudder.poll()
# timeout sources
t = time.monotonic()
for name in self.sensors:
sensor = self.sensors[name]
if sensor.source.value == 'none':
continue
if t - sensor.lastupdate > 8:
self.lostsensor(sensor)
def lostsensor(self, sensor):
print('sensor', sensor.name, 'lost', sensor.source.value,
sensor.device)
sensor.source.set('none')
sensor.reset()
sensor.device = None
def lostgpsd(self):
if self.gps.source.value == 'gpsd':
self.lostsensor(self.gps)
def write(self, sensor, data, source):
if not sensor in self.sensors:
print('unknown data parsed!', sensor)
return
self.sensors[sensor].write(data, source)
def lostdevice(self, device):
# optional routine useful when a device is
# unplugged to skip the normal data timeout
for name in self.sensors:
sensor = self.sensors[name]
if sensor.device and sensor.device[2:] == device:
self.lostsensor(sensor)
class Autopilot(object):
def __init__(self):
super(Autopilot, self).__init__()
# setup all processes to exit on any signal
self.childpids = []
def cleanup(signal_number, frame=None):
print 'got signal', signal_number, 'cleaning up'
while self.childpids:
pid = self.childpids.pop()
#print 'kill child', pid
os.kill(pid, signal.SIGTERM) # get backtrace
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)
# self.server = SignalKServer()
self.server = SignalKPipeServer()
self.boatimu = BoatIMU(self.server)
self.servo = servo.Servo(self.server)
self.nmea = Nmea(self.server)
self.version = self.Register(JSONValue, 'version',
'pypilot' + ' ' + strversion)
self.heading_command = self.Register(HeadingProperty,
'heading_command', 0)
self.enabled = self.Register(BooleanProperty, 'enabled', False)
self.lost_mode = self.Register(BooleanValue, 'lost_mode', False)
self.mode = self.Register(ModeProperty, 'mode')
self.mode.ap = self
self.lastmode = False
self.last_heading = False
self.last_heading_off = self.boatimu.heading_off.value
self.pilots = []
for pilot_type in pilots.default:
self.pilots.append(pilot_type(self))
#names = map(lambda pilot : pilot.name, self.pilots)
self.pilot = self.Register(EnumProperty, 'pilot', 'basic',
['simple', 'basic', 'learning'])
timestamp = self.server.TimeStamp('ap')
self.heading = self.Register(SensorValue,
'heading',
timestamp,
directional=True)
self.heading_error = self.Register(SensorValue, 'heading_error',
timestamp)
self.heading_error_int = self.Register(SensorValue,
'heading_error_int', timestamp)
self.heading_error_int_time = time.time()
self.tack = tacking.Tack(self)
self.gps_compass_offset = self.Register(SensorValue,
'gps_offset',
timestamp,
directional=True)
self.gps_speed = self.Register(SensorValue, 'gps_speed', timestamp)
self.wind_compass_offset = self.Register(SensorValue,
'wind_offset',
timestamp,
directional=True)
self.true_wind_compass_offset = self.Register(SensorValue,
'true_wind_offset',
timestamp,
directional=True)
self.wind_direction = self.Register(SensorValue,
'wind_direction',
timestamp,
directional=True)
self.wind_speed = self.Register(SensorValue, 'wind_speed', timestamp)
self.runtime = self.Register(TimeValue, 'runtime') #, persistent=True)
device = '/dev/watchdog0'
self.watchdog_device = False
try:
self.watchdog_device = open(device, 'w')
except:
print 'warning: failed to open special file', device, 'for writing'
print ' cannot stroke the watchdog'
if os.system('sudo chrt -pf 1 %d 2>&1 > /dev/null' % os.getpid()):
print 'warning, failed to make autopilot process realtime'
self.starttime = time.time()
self.times = 4 * [0]
self.childpids = [
self.boatimu.imu_process.pid, self.boatimu.auto_cal.process.pid,
self.server.process.pid, self.nmea.process.pid,
self.nmea.gpsdpoller.process.pid
]
signal.signal(signal.SIGCHLD, cleanup)
import atexit
atexit.register(lambda: cleanup('atexit'))
self.lastdata = False
self.lasttime = time.time()
# read initial value from imu as this takes time
# while not self.boatimu.IMURead():
# time.sleep(.1)
def __del__(self):
print 'closing autopilot'
self.server.__del__()
if self.watchdog_device:
print 'close watchdog'
self.watchdog_device.write('V')
self.watchdog_device.close()
def Register(self, _type, name, *args, **kwargs):
return self.server.Register(
_type(*(['ap.' + name] + list(args)), **kwargs))
def run(self):
while True:
self.iteration()
def mode_lost(self, newmode):
self.mode.update(newmode)
self.lost_mode.update(True)
def mode_found(self):
self.mode.set(self.lastmode)
self.lost_mode.set(False)
def iteration(self):
data = False
t00 = time.time()
for tries in range(14): # try 14 times to read from imu
data = self.boatimu.IMURead()
if data:
break
time.sleep(self.boatimu.period / 10)
if not data and self.lastdata:
print 'autopilot failed to read imu at time:', time.time()
self.lastdata = data
t0 = time.time()
dt = t0 - self.lasttime
self.lasttime = t0
self.server.TimeStamp('ap', time.time() - self.starttime)
compass_heading = self.boatimu.SensorValues['heading_lowpass'].value
headingrate = self.boatimu.SensorValues['headingrate_lowpass'].value
#switch back to the last mode if possible
if self.lost_mode.value and self.lastmode in self.nmea.values and \
self.nmea.values[self.lastmode]['source'].value != 'none':
mode_found()
#update wind and gps offsets
if self.nmea.values['gps']['source'].value != 'none':
d = .002
gps_speed = self.nmea.values['gps']['speed'].value
self.gps_speed.set((1 - d) * self.gps_speed.value + d * gps_speed)
if gps_speed > 1: # don't update gps offset below 1 knot
# weight gps compass offset higher with more gps speed
d = .005 * math.log(gps_speed + 1)
gps_compass_offset = resolv(
self.nmea.values['gps']['track'].value - compass_heading,
self.gps_compass_offset.value)
self.gps_compass_offset.set(
resolv(d * gps_compass_offset +
(1 - d) * self.gps_compass_offset.value))
if self.nmea.values['wind']['source'].value != 'none':
d = .005
wind_speed = self.nmea.values['wind']['speed'].value
self.wind_speed.set((1 - d) * self.wind_speed.value +
d * wind_speed)
# weight wind direction more with higher wind speed
d = .005 * math.log(wind_speed / 5.0 + .2)
if d > 0: # below 4 knots of wind, can't even use it
wind_direction = resolv(
self.nmea.values['wind']['direction'].value,
self.wind_direction.value)
wind_direction = (
1 - d) * self.wind_direction.value + d * wind_direction
self.wind_direction.set(resolv(wind_direction, 180))
offset = resolv(wind_direction + compass_heading,
self.wind_compass_offset.value)
self.wind_compass_offset.set(
resolv(d * offset +
(1 - d) * self.wind_compass_offset.value))
if self.mode.value == 'true wind':
# for true wind, we must have both wind and gps
if self.nmea.values['wind']['source'].value == 'none':
self.mode_lost('gps')
elif self.nmea.values['gps']['source'].value == 'none':
self.mode_lost('wind')
wind_speed = self.wind_speed.value
if wind_speed < 1: # below 1 knots wind speed, not reliable
self.mode_lost('gps')
gps_speed = self.gps_speed.value
if gps_speed < 1:
self.mode_lost('wind')
rd = math.radians(self.wind_direction.value)
windv = wind_speed * math.sin(rd), wind_speed * math.cos(rd)
truewindd = math.degrees(math.atan2(windv[0],
windv[1] - gps_speed))
offset = resolv(truewindd + compass_heading,
self.true_wind_compass_offset.value)
d = .005
self.true_wind_compass_offset.set(
resolv(d * offset +
(1 - d) * self.true_wind_compass_offset.value))
true_wind = resolv(
self.true_wind_compass_offset.value - compass_heading, 180)
self.heading.set(true_wind)
if self.mode.value == 'wind':
# if wind sensor drops out, switch to compass
if self.nmea.values['wind']['source'].value == 'none':
self.mode_lost('compass')
wind_direction = resolv(
self.wind_compass_offset.value - compass_heading, 180)
self.heading.set(wind_direction)
if self.mode.value == 'gps':
# if gps drops out switch to compass
if self.nmea.values['gps']['source'].value == 'none':
self.mode_lost('compass')
gps_heading = resolv(
compass_heading + self.gps_compass_offset.value, 180)
self.heading.set(gps_heading)
if self.mode.value == 'compass':
if data:
if 'calupdate' in data and self.last_heading:
# with compass calibration updates, adjust the autopilot heading_command
# to prevent actual course change
self.last_heading = resolv(self.last_heading,
data['heading'])
heading_off = data[
'heading'] - headingrate * dt - self.last_heading
self.heading_command.set(
resolv(self.heading_command.value + heading_off, 180))
self.last_heading = data['heading']
# if heading offset alignment changed, keep same course
if self.last_heading_off != self.boatimu.heading_off.value:
self.last_heading_off = resolv(self.last_heading_off,
self.boatimu.heading_off.value)
heading_off = self.boatimu.heading_off.value - self.last_heading_off
self.heading_command.set(
resolv(self.heading_command.value + heading_off, 180))
self.last_heading_off = self.boatimu.heading_off.value
self.heading.set(compass_heading)
if self.enabled.value:
if self.mode.value != self.lastmode: # mode changed?
if self.lastmode:
err = self.heading_error.value
if 'wind' in self.mode.value:
err = -err
else:
err = 0 # no error if enabling
self.heading_command.set(resolv(self.heading.value - err, 180))
self.runtime.update()
self.servo.servo_calibration.stop()
else:
self.runtime.stop()
self.lastmode = False
# filter the incoming heading and gyro heading
# error +- 60 degrees
heading = self.heading.value
heading_command = self.heading_command.value
err = minmax(resolv(heading - heading_command), 60)
# since wind direction is where the wind is from, the sign is reversed
if 'wind' in self.mode.value:
err = -err
self.heading_error.set(err)
t = time.time()
# compute integral
dt = t - self.heading_error_int_time
dt = max(min(dt, 1), 0) # ensure dt is from 0 to 1
self.heading_error_int_time = t
# int error +- 1, from 0 to 1500 deg/s
self.heading_error_int.set(minmax(self.heading_error_int.value + \
(self.heading_error.value/1500)*dt, 1))
if not self.tack.process():
for pilot in self.pilots:
if pilot.name == self.pilot.value:
pilot.process_imu_data() # implementation specific process
break
# servo can only disengauge under manual control
self.servo.force_engaged = self.enabled.value
self.lastmode = self.mode.value
t1 = time.time()
if t1 - t0 > self.boatimu.period / 2:
print 'Autopilot routine is running too _slowly_', t1 - t0, BoatIMU.period / 2
self.servo.poll()
t2 = time.time()
if t2 - t1 > self.boatimu.period / 2:
print 'servo is running too _slowly_', t2 - t1
self.nmea.poll()
t4 = time.time()
if t4 - t2 > self.boatimu.period / 2:
print 'nmea is running too _slowly_', t4 - t2
self.server.HandleRequests()
t5 = time.time()
if t5 - t4 > self.boatimu.period / 2:
print 'server is running too _slowly_', t5 - t4
times = t1 - t0, t2 - t1, t4 - t2
#self.times = map(lambda x, y : .975*x + .025*y, self.times, times)
#print 'times', map(lambda t : '%.2f' % (t*1000), self.times)
if self.watchdog_device:
self.watchdog_device.write('c')
while True:
dt = self.boatimu.period - (time.time() - t00)
if dt <= 0 or dt >= self.boatimu.period:
break
time.sleep(dt)