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')