def __init__(self, server, *args, **keywords):
self.server = server
self.loopfreq = self.Register(LoopFreqValue, 'loopfreq', 0)
self.alignmentQ = self.Register(QuaternionValue, 'alignmentQ', [1, 0, 0, 0], persistent=True)
self.heading_off = self.Register(RangeProperty, 'heading_offset', 0, -180, 180, persistent=True)
self.alignmentCounter = self.Register(Property, 'alignmentCounter', 0)
self.last_alignmentCounter = False
self.uptime = self.Register(TimeValue, 'uptime')
self.compass_calibration_age = self.Register(AgeValue, 'compass_calibration_age', persistent=True)
self.compass_calibration = self.Register(RoundedValue, 'compass_calibration', [[0, 0, 0, 30, 0], [False, False], 0], persistent=True)
self.compass_calibration_sigmapoints = self.Register(RoundedValue, 'compass_calibration_sigmapoints', False)
self.compass_calibration_locked = self.Register(BooleanProperty, 'compass_calibration_locked', False, persistent=True)
self.imu_pipe, imu_pipe = NonBlockingPipe('imu_pipe')
imu_cal_pipe = NonBlockingPipe('imu_cal_pipe')
self.poller = select.poll()
self.poller.register(self.imu_pipe, select.POLLIN)
self.compass_auto_cal = MagnetometerAutomaticCalibration(imu_cal_pipe[1], self.compass_calibration.value[0])
self.lastqpose = False
self.FirstTimeStamp = False
self.headingrate = self.heel = 0
self.heading_lowpass_constant = self.Register(RangeProperty, 'heading_lowpass_constant', .1, .01, 1)
self.headingrate_lowpass_constant = self.Register(RangeProperty, 'headingrate_lowpass_constant', .1, .01, 1)
self.headingraterate_lowpass_constant = self.Register(RangeProperty, 'headingraterate_lowpass_constant', .1, .01, 1)
sensornames = ['accel', 'gyro', 'compass', 'accelresiduals', 'pitch', 'roll']
sensornames += ['pitchrate', 'rollrate', 'headingrate', 'headingraterate', 'heel']
sensornames += ['headingrate_lowpass', 'headingraterate_lowpass']
directional_sensornames = ['heading', 'heading_lowpass']
sensornames += directional_sensornames
self.SensorValues = {}
timestamp = server.TimeStamp('imu')
for name in sensornames:
self.SensorValues[name] = self.Register(SensorValue, name, timestamp, directional = name in directional_sensornames)
# quaternion needs to report many more decimal places than other sensors
sensornames += ['fusionQPose']
self.SensorValues['fusionQPose'] = self.Register(SensorValue, 'fusionQPose', timestamp, fmt='%.7f')
sensornames += ['gyrobias']
self.SensorValues['gyrobias'] = self.Register(SensorValue, 'gyrobias', timestamp, persistent=True)
self.imu_process = multiprocessing.Process(target=imu_process, args=(imu_pipe,imu_cal_pipe[0], self.compass_calibration.value[0], self.SensorValues['gyrobias'].value))
self.imu_process.start()
self.last_imuread = time.time()
self.lasttimestamp = 0
self.last_heading_off = 3000 # invalid
def __init__(self, cal_pipe, current):
self.cal_pipe = cal_pipe
self.sphere_fit = current
points, self.points = NonBlockingPipe('points pipe', True)
norm_pipe, self.norm_pipe = NonBlockingPipe('norm pipe', True)
self.fit_output, fit_output = NonBlockingPipe('fit output', True)
self.process = multiprocessing.Process(target=CalibrationProcess,
args=(points, norm_pipe,
fit_output,
self.sphere_fit))
#print 'start cal process'
self.process.start()
def initialize(self):
# Build model
self.history = History()
self.learning_pipe, pipe = NonBlockingPipe('learning_pipe')
model_pipe, self.model_pipe = NonBlockingPipe('learning_model_pipe')
self.model_pipe_poller = select.poll()
self.model_pipe_poller.register(pipe, select.POLLIN)
self.fit_process = multiprocessing.Process(target=LearningProcess,
args=(pipe, model_pipe))
self.fit_process.start()
print('start training')
self.initialized = True
class MagnetometerAutomaticCalibration(object):
def __init__(self, cal_pipe, current):
self.cal_pipe = cal_pipe
self.sphere_fit = current
points, self.points = NonBlockingPipe('points pipe', True)
norm_pipe, self.norm_pipe = NonBlockingPipe('norm pipe', True)
self.fit_output, fit_output = NonBlockingPipe('fit output', True)
self.process = multiprocessing.Process(target=CalibrationProcess,
args=(points, norm_pipe,
fit_output,
self.sphere_fit))
#print 'start cal process'
self.process.start()
def __del__(self):
print 'terminate calibration process'
self.process.terminate()
def AddPoint(self, point):
self.points.send(point, False)
def SetNorm(self, norm):
self.norm_pipe.send(norm)
def UpdatedCalibration(self):
result = self.fit_output.recv()
if not result:
return
# use new bias fit
self.cal_pipe.send(tuple(result[0][0][:3]))
return result
class LearningPilot(AutopilotPilot):
def __init__(self, ap):
super(LearningPilot, self).__init__('learning', ap)
# create filters
timestamp = self.ap.server.TimeStamp('ap')
# create simple pid filter
self.P = self.Register(AutopilotGain, 'P', .001, .0001, .01)
self.D = self.Register(AutopilotGain, 'D', .03, .01, .1)
self.lag = self.Register(RangeProperty, 'lag', 1, 0, 5)
timestamp = self.ap.server.TimeStamp('ap')
self.dt = self.Register(SensorValue, 'dt', timestamp)
self.initialized = False
self.start_time = time.time()
self.model_uid = 0
def reset(self):
PreTrain(self.model)
def initialize(self):
# Build model
self.history = History()
self.learning_pipe, pipe = NonBlockingPipe('learning_pipe')
model_pipe, self.model_pipe = NonBlockingPipe('learning_model_pipe')
self.model_pipe_poller = select.poll()
self.model_pipe_poller.register(pipe, select.POLLIN)
self.fit_process = multiprocessing.Process(target=LearningProcess,
args=(pipe, model_pipe))
self.fit_process.start()
print('start training')
self.initialized = True
def process(self, reset):
ap = self.ap
if not self.initialized:
if time.time() - self.start_time < 2:
return
self.initialize()
P = ap.heading_error.value
D = ap.boatimu.SensorValues['headingrate_lowpass'].value
accel = ap.boatimu.SensorValues['accel'].value
gyro = ap.boatimu.SensorValues['gyro'].value
wind = ap.sensors.wind
# input data
data = [P, D] + list(accel) + list(gyro)
data += [ap.servo.voltage.value / 24, ap.servo.current.value / 20]
data += [wind.direction.value / 360, wind.speed.value / 60]
# training data
lag_samples = int(self.lag.value / self.ap.boatimu.rate.value)
self.history.put(data, samples + lag_samples)
learning_history = self.history.data[lag_samples:]
if len(learning_history) == samples:
#error = 0
#for d in self.history.data[:lag_samples]:
# error += d[0]*self.P.value + d[1]*self.D.value # calculate error from current state
#error /= lag_samples
d = self.history.data[0]
e = d[0] * self.P.value + d[
1] * self.D.value # calculate error from current state
# see what our command was to find the better command
data = {
'input': learning_history[0],
'error': e,
'uid': self.model_uid
}
self.learning_pipe.send(data)
history = self.history.data[:samples]
if len(history) == samples:
if self.model_pipe_poller.poll():
tflite_model, self.model_uid = self.model_pipe.recv()
open('converted.tflite', 'wb').write(tflite_model)
if self.model_uid:
t0 = time.time()
interpreter = tf.lite.Interpreter(
model_path="converted.tflite")
t1 = time.time()
interpreter.allocate_tensors()
t2 = time.time()
input_details = interpreter.get_input_details()
output_details = interpreter.get_output_details()
t3 = time.time()
input_shape = input_details[0]['shape']
print('input details', input_details)
t4 = time.time()
interpreter.set_tensor(input_details[0]['index'],
np.array(history))
interpreter.invoke()
t5 = time.time()
output_data = interpreter.get_tensor(
output_details[0]['index'])
t6 = time.time()
print('interpreter timings', t1 - t0, t2 - t1, t3 - t2,
t4 - t3, t5 - t4, t6 - t5)
if ap.enabled.value and len(self.history.data) >= samples:
ap.servo.command.set(command)
interpreter.invoke()
t5 = time.time()
output_data = interpreter.get_tensor(
output_details[0]['index'])
t6 = time.time()
print('interpreter timings', t1 - t0, t2 - t1, t3 - t2,
t4 - t3, t5 - t4, t6 - t5)
if ap.enabled.value and len(self.history.data) >= samples:
ap.servo.command.set(command)
pilot = LearningPilot
if __name__ == '__main__':
learning_pipe, pipe = NonBlockingPipe('learning_pipe')
fit_process = multiprocessing.Process(target=LearningProcess,
args=(pipe, ))
fit_process.start()
x = 0
while True:
P = math.sin(x)
D = math.sin(x + 3)
x += .01
inp = [0] * num_inputs
inp[0] = P
inp[1] = D
error = math.sin(x - 1)
data = {'input': inp, 'error': error}
