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)
