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 +