def new(): # https://stackoverflow.com/a/735978/3339274 time = (datetime.datetime.now() - START_TIME).total_seconds() acc = [ IMU.readACCx() * IMU_ACC_COEFF, -IMU.readACCy() * IMU_ACC_COEFF, -IMU.readACCz() * IMU_ACC_COEFF ] # Compensate for gravitational acceleration. # TODO: Do we want to do some angle calculations to figure out how much to subtract from x, y, and z? acc[0] += 9.81 gyro = [ IMU.readGYRx() * IMU_GYRO_COEFF, IMU.readGYRy() * IMU_GYRO_COEFF, IMU.readGYRz() * IMU_GYRO_COEFF ] # TODO: Conversion needed? mag = [IMU.readMAGx(), IMU.readMAGy(), IMU.readMAGz()] baro_tuple = barometer.get_baro_values(i2c_bus) baro_p = baro_tuple[0] baro_temp = baro_tuple[1] return Timestep(time=time, acc=acc, gyro=gyro, mag=mag, baro_p=baro_p, baro_temp=baro_temp)
def __init__(self, flight_state, time=None, acc=None, gyro=None, mag=None, baro=None): self.flight_state = flight_state if (time == None): if (IMUData.start_time == None): IMUData.start_time = datetime.datetime.now() self.time = (datetime.datetime.now() - IMUData.start_time).total_seconds() else: self.time = time if (acc == None): self.acc = [ IMU.readACCx() * IMU_ACC_COEFF, -IMU.readACCy() * IMU_ACC_COEFF, -IMU.readACCz() * IMU_ACC_COEFF ] # Compensate for gravitational acceleration. # TODO: Do we want to do some angle calculations to figure out how much to subtract from x, y, and z? self.acc[0] += 9.81 else: self.acc = acc if (gyro == None): self.gyro = [ IMU.readGYRx() * IMU_GYRO_COEFF, IMU.readGYRy() * IMU_GYRO_COEFF, IMU.readGYRz() * IMU_GYRO_COEFF ] else: self.gyro = gyro if (mag == None): # TODO: Conversion needed? self.mag = [IMU.readMAGx(), IMU.readMAGy(), IMU.readMAGz()] else: self.mag = mag if (baro == None): tuple = barometer.get_baro_values(i2c_bus) self.baro = [tuple[0], tuple[1]] else: self.baro = baro # Start events list off as empty. self.events = []
def read_sensors(self): time = (datetime.datetime.now() - START_TIME).total_seconds() acc = [ IMU.readACCx() * IMU_ACC_COEFF, -IMU.readACCy() * IMU_ACC_COEFF, -IMU.readACCz() * IMU_ACC_COEFF ] # Compensate for gravitational acceleration. # TODO: Do we want to do some angle calculations to figure out how much to subtract from x, y, and z? acc[0] += 9.81 gyro = [ IMU.readGYRx() * IMU_GYRO_COEFF, IMU.readGYRy() * IMU_GYRO_COEFF, IMU.readGYRz() * IMU_GYRO_COEFF ] # TODO: Conversion needed? mag = [IMU.readMAGx(), IMU.readMAGy(), IMU.readMAGz()] self.filter.add_data(acc, gyro, mag) filtered = self.filter.update_filter() baro_tuple = barometer.get_baro_values(i2c_bus) baro_p = baro_tuple[0] baro_temp = baro_tuple[1] self.data = Timestep(time=time, acc=acc, gyro=gyro, mag=mag, acc_f=filtered[0], gyro_f=filtered[1], mag_f=filtered[2], baro_p=baro_p, baro_temp=baro_temp)
file = csv.writer(open(folder_path + file_root + str(file_suffix) + ".csv", 'w'), delimiter=',') file.writerow(["time (s)", "accX", "accY", "accZ", "gyroX", "gyroY", "gyroZ", "magX", "magY", "magZ", "baroTemp", "baroPressure", "accX Filter", "accY Filter", "accZ Filter", "gyroX Filter", "gyroY Filter", "gyroZ Filter", "magX Filter", "magY Filter", "magZ Filter"]) IMU.detectIMU() # Detect if BerryIMUv1 or BerryIMUv2 is connected. IMU.initIMU() # Initialise the accelerometer, gyroscope and compass try: filter = mean_filter.IMUFilter(10) start_time = datetime.datetime.now() while True: baroValues = baro.get_baro_values(bus) # Units: g's acc = [IMU.readACCx() * IMU_ACC_COEFF, -IMU.readACCy() * IMU_ACC_COEFF, -IMU.readACCz() * IMU_ACC_COEFF] # Units: deg/s gyro = [IMU.readGYRx() * IMU_GYRO_COEFF, IMU.readGYRy() * IMU_GYRO_COEFF, IMU.readGYRz() * IMU_GYRO_COEFF] # Units: ? mag = [IMU.readMAGx(), IMU.readMAGy(), IMU.readMAGz()] filter.add_data(acc, gyro, mag) clean = filter.update_filter() print(form((datetime.datetime.now() - start_time).total_seconds(), 4) + DEL +