def main(): sensorfusion = Kalman() print("Connecting to sensortag...") tag = SensorTag(macAddress) print("connected.") tag.accelerometer.enable() tag.magnetometer.enable() tag.gyroscope.enable() tag.battery.enable() time.sleep(1.0) # Loading sensors prev_time = time.time() while True: accelerometer_readings = tag.accelerometer.read() gyroscope_readings = tag.gyroscope.read() magnetometer_readings = tag.magnetometer.read() ax, ay, az = accelerometer_readings gx, gy, gz = gyroscope_readings mx, my, mz = magnetometer_readings curr_time = time.time() dt = curr_time - prev_time sensorfusion.computeAndUpdateRollPitchYaw(ax, ay, az, gx, gy, gz, mx, my, mz, dt) print(f"dt: {dt} pitch: {sensorfusion.pitch}") prev_time = curr_time print("Battery: ", tag.battery.read())
class BlueToothThreading: """ Bluetooth Threading The run() method will be started and it will run in the background until the application exits. """ def __init__(self, interval=1): """ Constructor :type interval: int :param interval: Check interval, in seconds """ self.sensorfusion = Kalman() print("Connecting to sensortag...") self.tag = SensorTag(macAddress) print("connected.") self.tag.accelerometer.enable() self.tag.magnetometer.enable() self.tag.gyroscope.enable() self.tag.battery.enable() self.pitch = 0 time.sleep(1.0) # Loading sensors self.prev_time = time.time() thread = threading.Thread(target=self.run, args=()) thread.daemon = True # Daemonize thread thread.start() # Start the execution def run(self): """ Method that runs forever """ while True: accelerometer_readings = self.tag.accelerometer.read() gyroscope_readings = self.tag.gyroscope.read() magnetometer_readings = self.tag.magnetometer.read() ax, ay, az = accelerometer_readings gx, gy, gz = gyroscope_readings mx, my, mz = magnetometer_readings curr_time = time.time() dt = curr_time - self.prev_time self.sensorfusion.computeAndUpdateRollPitchYaw( ax, ay, az, gx, gy, gz, mx, my, mz, dt) self.pitch = self.sensorfusion.pitch self.prev_time = curr_time print("Battery: ", self.tag.battery.read())
class BlueToothThreading: """ Bluetooth Threading The run() method will be started and it will run in the background until the application exits. """ def __init__(self, interval=1): """ Constructor :type interval: int :param interval: Check interval, in seconds """ self.sensorfusion = Kalman() print("Connecting to sensortag...") self.tag = SensorTag(macAddress) print("connected.") self.tag.accelerometer.enable() self.tag.magnetometer.enable() self.tag.gyroscope.enable() self.tag.battery.enable() self.pitch = 0 self.angular_velocity = 0 self.linear_velocity = 0 self.acceleration = 0 time.sleep(1.0) # Loading sensors self.prev_time = time.time() thread = threading.Thread(target=self.run, args=()) thread.daemon = True # Daemonize thread thread.start() # Start the execution def run(self): """ Method that runs forever """ while True: accelerometer_readings = self.tag.accelerometer.read() gyroscope_readings = self.tag.gyroscope.read() magnetometer_readings = self.tag.magnetometer.read() ax, ay, az = accelerometer_readings gx, gy, gz = gyroscope_readings mx, my, mz = magnetometer_readings curr_time = time.time() dt = curr_time - self.prev_time self.sensorfusion.computeAndUpdateRollPitchYaw( ax, ay, az, gx, gy, gz, mx, my, mz, dt) pitch = self.sensorfusion.pitch * 0.0174533 dp = pitch - self.pitch da = ax - self.acceleration self.angular_velocity = dp / dt self.linear_velocity = da * dt self.pitch = pitch self.acceleration = ax self.prev_time = curr_time print("Battery: ", self.tag.battery.read()) def take_observation(self): return [ self.pitch, self.angular_velocity, self.linear_velocity, self.acceleration ] # return observation with manual calbiration # expects list of length STATE_SIZE def take_observation_calibrated(self, calibration): return [ self.pitch - calibration[0], self.angular_velocity - calibration[1], self.linear_velocity - calibration[2], self.acceleration - calibration[3] ]
imu.read_sensors() imu.computeOrientation() sensorfusion.roll = imu.roll sensorfusion.pitch = imu.pitch sensorfusion.yaw = imu.yaw currTime = time.time() while True: imu.read_sensors() imu.computeOrientation() print( f"roll: {round(imu.roll,3)}, pitch: {round(imu.pitch,3)}, yaw: {round(imu.yaw,3)}" ) newTime = time.time() dt = newTime - currTime currTime = newTime sensorfusion.computeAndUpdateRollPitchYaw( imu.accel_data[0], imu.accel_data[1], imu.accel_data[2], imu.gyro_data[0], imu.gyro_data[1], imu.gyro_data[2], imu.magneto_data[0], imu.magneto_data[1], imu.magneto_data[2], dt) print( f"kalman roll: {round(sensorfusion.roll,3)}, kalman pitch: {round(sensorfusion.pitch,3)}, kalman yaw: {round(sensorfusion.yaw,3)}, delta_time: {round(dt,3)}" ) #time.sleep(0.1)