def plot_gyro():
global csv, timeArr, orientation
import ahrs.common.orientation as orient
euler = []
for row in orientation.Q:
# Roll is about x, pitch about y and yaw is about z
# The coordinate system appears to be NED (North-East-Down)??
# I know for sure Z axis is vertical
euler.append(orient.q2rpy(row))
euler = np.array(euler)
euler = euler * RAD2DEG
plt.figure()
plt.subplot(211)
# plt.plot(timeArr, euler[:,0], label="Roll, deg")
# plt.plot(timeArr, euler[:,1], label="Pitch, deg")
# plt.plot(timeArr, euler[:,2], label="Yaw, deg")
plt.plot(euler[:, 0], label="Roll, deg")
plt.plot(euler[:, 1], label="Pitch, deg")
plt.plot(euler[:, 2], label="Yaw, deg")
# plt.plot(csv["imu1_accel_y_real"], label="Y accel")
# plt.plot(csv["pos_z"], label="Altitude")
plt.legend()
plt.subplot(212)
# we can really just plot the orientation direction cosines
cosines = [orient.q2euler(row) for row in orientation.Q]
cosines = np.array(cosines)
plt.plot(timeArr, cosines[:, 0], label="East")
plt.plot(timeArr, cosines[:, 1], label="North")
plt.plot(timeArr, cosines[:, 2], label="Up")
# plt.plot(timeArr, csv['ax'], label="Ax, gees")
# plt.plot(timeArr, csv['ay'], label="Ay, gees")
# plt.plot(timeArr, csv['az'], label="Az, gees")
plt.legend()
gx = 0
if abs(gy) < 5:
gy = 0
if abs(gz) < 6:
gz = 0
gx = gx * pi / 180
gy = gy * pi / 180
gz = gz * pi / 180
#Update the state for EKF using quaternion
acce = np.array([ax, ay, az])
gyo = np.array([gx, gy, gz])
mag = np.array([x, y, z])
orientation.Dt = dt
Q = orientation.update(gyo, acce, mag, Q)
pitch, roll, yaw = q2euler(Q)
# Get accelerometer measurements and remove offsets
[phi_acc, theta_acc] = imu.get_acc_angles()
# Gey gyro measurements and calculate Euler angle derivatives
phi_dot_0 = gx + sin(phi_hat) * tan(theta_hat) * gy + cos(phi_hat) * tan(
theta_hat) * gz
theta_dot = cos(phi_hat) * gy - sin(phi_hat) * gz
# Kalman filter
A = np.array([[1, -dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, -dt], [0, 0, 0, 1]])
B = np.array([[dt, 0], [0, 0], [0, dt], [0, 0]])
gyro_input = np.array([[phi_dot_0], [theta_dot]])
gx = 0
if abs(gy) < 5:
gy = 0
if abs(gz) < 6:
gz = 0
gx = gx * pi / 180
gy = gy * pi / 180
gz = gz * pi / 180
#Update the state for EKF using quaternion
acce = np.array([ax, ay, az])
gyo = np.array([gx, gy, gz])
mag = np.array([x, y, z])
orientation.Dt = dt
Quater = orientation.update(gyo, acce, mag, Quater)
pitch, roll, yaw = q2euler(Quater)
# Get accelerometer measurements and remove offsets
[phi_acc, theta_acc] = mpu_1.get_acc_angles()
# Gey gyro measurements and calculate Euler angle derivatives
phi_dot_0 = gx + sin(phi_hat) * tan(theta_hat) * gy + cos(phi_hat) * tan(
theta_hat) * gz
theta_dot = cos(phi_hat) * gy - sin(phi_hat) * gz
# Kalman filter
A = np.array([[1, -dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, -dt], [0, 0, 0, 1]])
B = np.array([[dt, 0], [0, 0], [0, dt], [0, 0]])
gyro_input = np.array([[phi_dot_0], [theta_dot]])
def _update_loop(self):
logger.debug("Starting _update_loop")
filter = Madgwick(frequency=50, beta=0.1)
# filter = Mahony()
q = [1, 0, 0, 0]
yaw_readings = deque([])
yaw_buffer_size = 500
self.is_updating = True
while self.is_updating:
try:
accel = self.mpu9250.readAccelerometerMaster()
gyro = list(
map(lambda x: x * DEG2RAD, self.mpu9250.readGyroscopeMaster())
)
mag = self.mpu9250.readMagnetometerMaster()
self.yaw_raw = math.atan2(-mag[1], mag[0]) * RAD2DEG
# @TODO: Roll and Pitch raw headings
q = filter.updateMARG(
acc=accel, gyr=gyro, mag=[mag[0], -mag[1], mag[2]], q=q
)
attitude = q2euler(q)
self.roll_filtered = attitude[0] * RAD2DEG
self.pitch_filtered = attitude[1] * RAD2DEG
self.yaw_filtered = attitude[2] * RAD2DEG
self.has_position = True
# Low pass filter for yaw readings
# Also used to determine if the current yaw reading is 'stable' or not
yaw_readings.append(self.yaw_filtered)
if len(yaw_readings) >= yaw_buffer_size:
yaw_readings.popleft()
yaw_numpy = numpy.array(yaw_readings)
self.yaw_smoothed = yaw_numpy.mean()
self.position_stable = (
True if (yaw_numpy.std() < self.stability_threshold) else False
)
else:
self.position_stable = False
except Exception as ex:
logger.error(ex)
# @TODO: Roll and Pitch smoothed headings
time.sleep(0.0001)
# We have stopped updating, reset everything back to zero/off
self.has_position = False
self.position_stable = False
self.roll_raw = 0.0
self.pitch_raw = 0.0
self.yaw_raw = 0.0
self.roll_filtered = 0.0
self.pitch_filtered = 0.0
self.yaw_filtered = 0.0
self.roll_smoothed = 0.0
self.pitch_smoothed = 0.0
self.yaw_smoothed = 0.0
def show_rpy(v):
print('roll:{:+10.5f}'.format(v[0]), end=' ')
print('pitch:{:+10.5f}'.format(v[1]), end=' ')
print('yaw:{:+10.5f}'.format(v[2]))
def show_q(q):
print('w:{:+10.5f} x:{:+10.5f} y:{:+10.5f} z:{:+10.5f}'.format(*q))
try:
imu = mpu9250()
mw = Madgwick(frequency=50, beta=1)
dcc = DynamicCompassCalibrator()
q = [1, 0, 0, 0]
while True:
a = list(imu.accel)
g = list(map(lambda x: x * DEG2RAD, imu.gyro))
m = list(imu.mag)
#m = dcc.correct(m)
q = mw.updateIMU(g, a, q)
#q = mw.updateMARG(g, a, [m[1], m[0], -m[2]], q)
#show_q(q)
a = rad2deg(q2euler(q))
show_rpy(a)
sleep(0.02)
except KeyboardInterrupt:
print('bye ...')