def run(model_dir): acce_operation = Acce_operation(model_dir) est = Estimator() a.write(b'\x01') window_data = np.array([[0 for i in range(TOTAL_WINDOW_SIZE)], [0 for i in range(TOTAL_WINDOW_SIZE)], [0 for i in range(TOTAL_WINDOW_SIZE)]], dtype=np.float32) index = 0 sample_num = 0 lastTp = time.time() while True: data = a.read(14) raw_acc = read_data(data, 0) raw_gyr = read_data(data, 8) tp = time.time() dt = tp - lastTp lastTp = tp # attitude estimation vp = est.feed_data(dt, raw_gyr, raw_acc) # print(vp, end="\r") for i in range(3): window_data[i][index] = vp[i] index += 1 sample_num += 1 if index == TOTAL_WINDOW_SIZE: index = 0 if sample_num == SAMPLE_SIZE: sample_num = 0 operate(acce_operation, window_data, index)
def run(acc_operation): est = Estimator() a.write(b'\x01') lastTp = time.time() while True: data = a.read(14) raw_acc = read_data(data, 0) raw_gyr = read_data(data, 8) # print(data) tp = time.time() dt = tp - lastTp lastTp = tp # attitude estimation vp = est.feed_data(dt, raw_gyr, raw_acc) # print(vp, end="\r") for i in range(3): vp[i] = vp[i] * TIMES # print(vp) ret, logits = acc_operation.feed_data(vp) operation(ret, logits)
class acc_module(threading.Thread): def __init__(self, feed_data_func): threading.Thread.__init__(self) self.feed_data = feed_data_func self.serial_port = serial.Serial('/dev/ttyACM0', 115200) self.est = Estimator() def run(self): lastTp = time.time() while True: data = self.serial_port.read(14) raw_acc = read_data(data, 0) raw_gyr = read_data(data, 8) tp = time.time() dt = tp - lastTp lastTp = tp # attitude estimation vp = self.est.feed_data(dt, raw_gyr, raw_acc) self.feed_data(vp)
def run(): pl.ion() pl.show() est = Estimator() a.write(b'\x01') window_data = np.array([[0 for i in range(WINDOW_SIZE)], [0 for i in range(WINDOW_SIZE)], [0 for i in range(WINDOW_SIZE)]]) pl.show() index = 0 sample_num = 0 lastTp = time.time() for i in range(3): line, = ax.plot(window_data[i]) lines.append(line) while True: data = a.read(14) raw_acc = read_data(data, 0) raw_gyr = read_data(data, 8) tp = time.time() dt = tp - lastTp lastTp = tp # attitude estimation vp = est.feed_data(dt, raw_gyr, raw_acc) print(vp, end="\r") for i in range(3): window_data[i][index] = vp[i] * 512 index += 1 sample_num += 1 if index == WINDOW_SIZE: index = 0 if sample_num == SAMPLE_SIZE: sample_num = 0 ret_data = np.concatenate( [window_data[:, index:], window_data[:, :index]], axis=1) plot(ret_data)
args = parser.parse_args() input_path = args.input_path output_path = args.output_path est_flag = args.estimator output_file_handle = open(output_path, 'w') est = Estimator(est_flag) with open(input_path) as file_handle: raw_data_list = file_handle.readlines() last_timestamp = -1 for raw_data_str in raw_data_list: raw_data_str = raw_data_str[:-1] raw_data = [x for x in raw_data_str.split(", ")] timestamp = float(raw_data[0]) acce_raw_data = [int(x) for x in raw_data[4:7]] gyro_data = [int(x) for x in raw_data[1:4]] if last_timestamp == -1: last_timestamp = timestamp continue acce_data = est.feed_data(timestamp - last_timestamp, gyro_data, acce_raw_data) new_acce_data = acce_data * TIMES ts = round(timestamp * 1000) # new_acce_data = [float(x) for x in new_acce_data] # print(timestamp - last_timestamp, acce_data, new_acce_data) output_file_handle.write( str(ts) + ' ' + str(new_acce_data[0]) + ' ' + str(new_acce_data[1]) + ' ' + str(new_acce_data[2]) + '\n') last_timestamp = timestamp output_file_handle.close()
est = Estimator() while True: # read data data = a.read(14) raw_acc = read_data(data, 0) raw_gyr = read_data(data, 8) # time interval between two samples (sec) tp = time.time() ts = round(tp * 1000) dt = tp - lastTp lastTp = tp # attitude estimation old_vp = est.feed_data(dt, raw_gyr, raw_acc) vp = old_vp * 4096 raw_datafile.write( str(tp) + ' ' + str(raw_gyr[0]) + ' ' + str(raw_gyr[1]) + ' ' + str(raw_gyr[2]) + ' ' + str(raw_acc[0]) + ' ' + str(raw_acc[1]) + ' ' + str(raw_acc[2]) + '\n') # datafile.write(str(ts) + ' ' + str(vp[0]) + ' ' + str(vp[1]) + ' ' + str(vp[2]) + '\n') print(old_vp, end='\r') count += 1 if count % 800 == 0: count = 0 # print("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f" % (acc[0], acc[1], acc[2], gyr[0], gyr[1], gyr[2], q[0], q[1], q[2], q[3]), # end='\r')