def accel_logger(timestamp):
""" Accel Data Logger """
global INLOCSYNC, DONE
max_x = max_y = max_z = -20
min_x = min_y = min_z = 20
sum_x = sum_y = sum_z = 0
c = 0
while not INLOCSYNC:
time.sleep(5)
output = open("/root/gps-data/%s_accel.csv" % timestamp, "w")
output.write("%s\n" % VERSION)
next_time = time.time() + 1
while not DONE:
try:
axes = accel.get_axes()
#put the axes into variables
x = axes['x']
y = axes['y']
z = axes['z']
if INLOCSYNC:
acceltime = datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%fZ")
output.write("%s A % 02.3f % 02.3f % 02.3f *\n" % (acceltime, x, y, z))
except KeyboardInterrupt:
DONE = True
except IOError:
pass
print "ACCEL done"
output.close()
def accel_logger(timestamp):
""" Accel Data Logger """
global INLOCSYNC
max_x = max_y = max_z = -20
min_x = min_y = min_z = 20
sum_x = sum_y = sum_z = 0
c = 0
while not INLOCSYNC:
time.sleep(5)
output = open("/root/gps-data/%s_accel.csv" % timestamp, "w")
output.write("%s\n" % VERSION)
next_time = time.time() + 1
while not DONE:
try:
axes = accel.get_axes()
#put the axes into variables
x = axes['x']
y = axes['y']
z = axes['z']
sum_x += x
sum_y += y
sum_z += z
max_x = max(x, max_x)
max_y = max(y, max_y)
max_z = max(z, max_z)
min_x = min(x, min_x)
min_y = min(y, min_y)
min_z = min(z, min_z)
c += 1
now = time.time()
if now > next_time and c != 0:
acceltime = time.strftime("%Y-%m-%dT%H:%M:%S.000Z")
x1 = sum_x / c
y1 = sum_y / c
z1 = sum_z / c
if INLOCSYNC:
output.write(
"%s A %d % 02.3f % 02.3f % 02.3f % 02.3f % 02.3f % 02.3f % 02.3f % 02.3f % 02.3f *\n"
% (acceltime, c, min_x, x1, max_x, min_y, y1, max_y,
min_z, z1, max_z))
max_x = max_y = max_z = -20
min_x = min_y = min_z = 20
sum_x = sum_y = sum_z = 0
c = 0
next_time = now + 1
except IOError:
pass
print "ACCEL done"
def imu_logger(timestamp):
""" Accel Data Logger """
global DONE, ACC_STATUS
device = accel.device()
max_x = max_y = max_z = -20
min_x = min_y = min_z = 20
sum_x = sum_y = sum_z = 0
c = 0
while not INLOCSYNC:
time.sleep(5)
output = open("/root/gps-data/%s_accel.csv" % timestamp, "w")
output.write("%s\n" % VERSION)
while not DONE:
#now = time.time()
try:
axes = accel.get_axes()
acceltime = datetime.datetime.now().strftime(
"%Y-%m-%dT%H:%M:%S.%fZ")
att_obj = {
"class": "ATT",
"device": device,
"time": acceltime,
"acc_x": axes['ACCx'],
"acc_y": axes['ACCy'],
"acc_z": axes['ACCz'],
"gyro_x": axes['GYRx'],
"gyro_y": axes['GYRy'],
"gyro_z": axes['GYRz'],
}
#put the axes into variables
if INLOCSYNC or ALWAYS_LOG:
output.write("%s ATT %s *\n" %
(acceltime, json.dumps(att_obj)))
ACC_STATUS = True
except KeyboardInterrupt:
DONE = True
except IOError:
pass
#while (time.time() - now < 0.02):
# time.sleep(0.001)
ACC_STATUS = False
print("ACCEL done")
output.close()
def imu_logger(output_directory):
""" IMU Logger """
global ATT
cf_angle_x = cf_angle_y = cf_angle_z = 0
config = util.read_config()
# Open the output file
with open(
os.path.join(
output_directory,
datetime.datetime.now().strftime("%Y%m%d%H%M") + "_imu.csv"),
"w") as imu_output:
imu_output.write("%s %s %s *\n" % (config['time'], "VERSION",
json.dumps({
"class": "VERSION",
"version": util.DATA_API
})))
imu_output.write("%s %s %s *\n" %
(config['time'], config['class'], json.dumps(config)))
now = time.time()
while not util.DONE:
last_time = now
now = time.time()
acc = accel.get_axes()
# Calibration
acc['MAGx'] -= (MAX_MAG_X + MIN_MAG_X) / 2
acc['MAGy'] -= (MAX_MAG_Y + MIN_MAG_Y) / 2
acc['MAGz'] -= (MAX_MAG_Z + MIN_MAG_Z) / 2
obj = {
"class": "ATT",
"device": accel.device(),
"time":
datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%fZ"),
"acc_x": acc['ACC' + config['imu']['x']],
"acc_y": acc['ACC' + config['imu']['y']],
"acc_z": acc['ACC' + config['imu']['z']],
"gyro_x": acc['GYR' + config['imu']['x']],
"gyro_y": acc['GYR' + config['imu']['y']],
"gyro_z": acc['GYR' + config['imu']['z']],
"mag_x": acc['MAG' + config['imu']['x']],
"mag_y": acc['MAG' + config['imu']['y']],
"mag_z": acc['MAG' + config['imu']['z']],
"temp": _get_temp(),
}
delta_time = now - last_time
# Calculate Yaw, Pitch and Roll with data fusion
acc_x_angle = math.degrees(math.atan2(obj['acc_y'], obj['acc_z']))
acc_y_angle = math.degrees(math.atan2(obj['acc_z'], obj['acc_x']))
acc_z_angle = math.degrees(math.atan2(obj['acc_x'], obj['acc_y']))
cf_angle_x = AA * (cf_angle_x + obj['gyro_x'] * delta_time) + (
1 - AA) * acc_x_angle
cf_angle_y = AA * (cf_angle_y + obj['gyro_y'] * delta_time) + (
1 - AA) * acc_y_angle
cf_angle_z = AA * (cf_angle_z + obj['gyro_z'] * delta_time) + (
1 - AA) * acc_z_angle
obj['pitch'] = cf_angle_x
obj['pitch_st'] = "N"
obj['roll'] = cf_angle_y - 90
obj['roll_st'] = "N"
obj['yaw'] = cf_angle_z
obj['yaw_st'] = "N"
# Calculate Heading
obj['heading'] = (math.degrees(
math.atan2(obj['mag_y'], obj['mag_x'])) - 90) % 360.0
# Calculate vector length
obj["acc_len"] = math.sqrt(obj['acc_x']**2 + obj['acc_y']**2 +
obj['acc_z']**2)
obj["mag_len"] = math.sqrt(obj['mag_x']**2 + obj['mag_y']**2 +
obj['mag_z']**2)
obj["mag_st"] = "N"
# Log the output
imu_output.write("%s %s %s *\n" %
(obj['time'], obj['class'], json.dumps(obj)))
ATT = obj
# Delay Loop
while (time.time() - now) < LOOP_DELAY:
time.sleep(SLEEP_TIME)