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)