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.sensors = Sensors(self.server)
self.servo = servo.Servo(self.server, self.sensors)
self.version = self.Register(Value, 'version',
'pypilot' + ' ' + strversion)
self.heading_command = self.Register(HeadingProperty,
'heading_command', 0)
self.enabled = self.Register(BooleanProperty, 'enabled', False)
self.lastenabled = 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))
self.pilot = self.Register(EnumProperty,
'pilot',
'basic',
['simple', 'basic', 'learning', 'wind'],
persistent=True)
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 = HeadingOffset()
self.gps_speed = self.Register(SensorValue, 'gps_speed', timestamp)
self.wind_compass_offset = HeadingOffset()
self.true_wind_compass_offset = HeadingOffset()
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.sensors.nmea.process.pid,
self.sensors.gps.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 switch_mode(self, newmode):
command = self.heading_command.value
mode_offsets = {
'compass': 0,
'gps': self.gps_compass_offset.value,
'wind': self.wind_compass_offset.value,
'true wind': self.true_wind_compass_offset.value
}
command -= mode_offsets[self.mode.value]
command += mode_offsets[newmode]
self.heading_command.set(command)
def mode_lost(self, newmode):
self.mode.update(newmode)
self.lost_mode.update(True)
def mode_changed(self):
if self.lastmode: # keep same course in new mode
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))
def recover_mode_lost(self):
#switch back to the lost mode if possible
if self.lost_mode.value and self.lastmode in self.sensors.sensors and \
self.sensors.sensors[self.lastmode].source.value != 'none':
self.mode.set(self.lastmode)
self.lost_mode.set(False)
def compute_offsets(self):
# compute difference between compass to gps and compass to wind
compass = self.boatimu.SensorValues['heading_lowpass'].value
if self.sensors.gps.source.value != 'none':
d = .002
gps_speed = self.sensors.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
gps_track = self.sensors.gps.track.value
# weight gps compass offset higher with more gps speed
d = .005 * math.log(gps_speed + 1)
self.gps_compass_offset.update(gps_track - compass, d)
if self.sensors.wind.source.value != 'none':
d = .005
wind_speed = self.sensors.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 = .05 * math.log(wind_speed / 5.0 + 1.2)
wind_direction = resolv(self.sensors.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))
self.wind_compass_offset.update(wind_direction + compass, d)
if self.sensors.gps.source.value != 'none':
self.true_wind = compute_true_wind(self.gps_speed.value,
self.wind_speed.value,
self.wind_direction)
offset = resolv(self.truewind + compass,
self.true_wind_compass_offset.value)
d = .05
self.true_wind_compass_offset.update(offset, d)
def fix_compass_calibration_change(self):
headingrate = self.boatimu.SensorValues['headingrate_lowpass'].value
t0 = time.time()
dt = t0 - self.lasttime
self.lasttime = t0
#if the compass gets a new fix, or the alignment changes,
# update the autopilot command so the course remains constant
self.compass_change = 0
data = self.lastdata
if data:
if 'calupdate' in data and self.last_heading:
# with compass calibration updates, adjust the autopilot heading_command
# to prevent actual course change
last_heading = resolv(self.last_heading, data['heading'])
self.compass_change += data[
'heading'] - headingrate * dt - last_heading
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)
self.compass_change += self.boatimu.heading_off.value - self.last_heading_off
self.last_heading_off = self.boatimu.heading_off.value
if self.compass_change:
self.gps_compass_offset.value -= self.compass_change
self.wind_compass_offset.value += self.compass_change
self.true_wind_compass_offset.value += self.compass_change
if self.mode.value == 'compass':
heading_command = self.heading_command.value + self.compass_change
self.heading_command.set(resolv(heading_command, 180))
def compute_heading_error(self):
# compute heading error
heading = self.heading.value
heading_command = self.heading_command.value
# error +- 60 degrees
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 for I gain
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))
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()
# set autopilot timestamp
self.server.TimeStamp('ap', time.time() - self.starttime)
self.recover_mode_lost()
self.fix_compass_calibration_change()
self.compute_offsets()
pilot = None
for p in self.pilots:
if p.name == self.pilot.value or not pilot:
pilot = p
pilot.compute_heading()
if self.enabled.value:
if self.mode.value != self.lastmode: # mode changed?
self.process_mode_changed()
self.runtime.update()
self.servo.servo_calibration.stop()
else:
self.runtime.stop()
self.lastmode = False
self.compute_heading_error()
# reset filters when autopilot is enabled
reset = False
if self.enabled.value != self.lastenabled:
self.lastenabled = self.enabled.value
if self.enabled.value:
self.heading_error_int.set(0) # reset integral
reset = True
# perform tacking or pilot specific calculation
if not self.tack.process():
pilot.process(reset) # implementation specific process
# servo can only disengage 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.sensors.poll()
t4 = time.time()
if t4 - t2 > self.boatimu.period / 2:
print('sensors 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)
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)