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}