def calc_orientation(gyr, acc, samplerate, align_arr, beta, delta_t, times):
ref_qq = np.zeros([len(acc), 4])
ref_eu = np.zeros([len(acc), 3])
sampleperiod = 1 / samplerate
qq = Quaternion(align_arr)
madgwick = MadgwickAHRS(sampleperiod=sampleperiod,
quaternion=qq,
beta=beta)
count = 0
ll = len(gyr)
for i in range(0, len(gyr) - 1):
if (i % 300) == 0:
print(
str(i) + ' / ' + str(ll) + ' ' + str(round(i / ll * 100, 0)) +
'%')
while (count < times):
madgwick.update_imu(gyr[i], acc[i], delta_t[i])
count += 1
count = 0
# ref_qq[i]=madgwick.quaternion.elements
# ref_eu[i]=madgwick.quaternion.eu_angle
ref_qq[i, 0] = madgwick.quaternion._get_q()[0]
ref_qq[i, 1] = madgwick.quaternion._get_q()[1]
ref_qq[i, 2] = madgwick.quaternion._get_q()[2]
ref_qq[i, 3] = madgwick.quaternion._get_q()[3]
tempq = Quaternion(ref_qq[i])
ref_eu[i, :] = tempq.to_euler_angles_by_wiki()
ref_eu[i] *= 180 / np.pi
return ref_qq, ref_eu
def calc_orientation_mag(gyr, acc, mag, samplerate, align_arr, beta, delta_t,
times):
ref_qq = np.zeros([len(acc), 4])
ref_eu = np.zeros([len(acc), 3])
# out_qq=np.zeros([len(acc),4])
# out_eu=np.zeros([len(acc),3])
sampleperiod = 1 / samplerate
qq = align_arr
madgwick = MadgwickAHRS(sampleperiod=sampleperiod,
quaternion=qq,
beta=beta)
rms_mag = np.zeros(len(mag))
for i in range(len(mag)):
rms_mag[i] = np.sqrt(
np.mean(mag[i, 0]**2 + mag[i, 1]**2 + mag[i, 2]**2))
count = 0
ll = len(gyr)
for i in range(0, len(gyr) - 1):
if (i % 2000) == 0:
print(
str(i) + ' / ' + str(ll) + ' ' + str(round(i / ll * 100, 1)) +
'%')
warnings.warn('mag maxium limit is on')
if (rms_mag[i] > 999):
print('warning!! magnetometer warning')
warnings.warn('rms_mag[i]>maxium')
while (count < times):
madgwick.update_imu(gyr[i], acc[i], delta_t[i])
count += 1
count = 0
else:
while (count < times):
madgwick.update(gyr[i], acc[i], mag[i], delta_t[i])
count += 1
count = 0
ref_qq[i, 0] = madgwick.quaternion._get_q()[0]
ref_qq[i, 1] = madgwick.quaternion._get_q()[1]
ref_qq[i, 2] = madgwick.quaternion._get_q()[2]
ref_qq[i, 3] = madgwick.quaternion._get_q()[3]
# ref_qq[i]=madgwick.quaternion.elements
tempq = Quaternion(ref_qq[i])
#
ref_eu[i, :] = tempq.to_euler_angles_by_wiki()
ref_eu[i] *= 180 / np.pi
# ref_eu[i]=madgwick.quaternion.eu_angle
#
return ref_qq, ref_eu
def main():
new_data = MadgwickAHRS()
while 1:
gyr = np.array(gyro.Get_CalOut_Value())
acc = np.array(accel.getAccel())
while not bool(magnet.isMagReady()):
print "not ready"
time.sleep(0.1)
mag = np.array(magnet.getMag())
gyr_rad = gyr * (np.pi / 180)
new_data.update_imu(gyr_rad, acc)
print(gyr, acc, mag)
print "---------------------------\n"
ahrs = new_data.quaternion.to_euler_angles()
print ahrs
time.sleep(1)
(gyro_scaled_x, gyro_scaled_y, gyro_scaled_z) = s.Get_CalOut_Value()
(accel_scaled_x, accel_scaled_y, accel_scaled_z) = ss.getAccel()
last_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
last_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
gyro_offset_x = gyro_scaled_x
gyro_offset_y = gyro_scaled_y
gyro_total_x = (last_x) - gyro_offset_x
gyro_total_y = (last_y) - gyro_offset_y
return gyro_total_x, gyro_total_y, gyro_offset_x, gyro_offset_y, last_x, last_y
if __name__ == "__main__":
new_data = MadgwickAHRS()
while 1:
gyr = np.array(s.Get_CalOut_Value())
acc = np.array(ss.getAccel())
print(gyr)
# while not bool(sm.isMagReady()):
# print "not ready"
# time.sleep(0.1)
# mag = np.array(sm.getMag())
gyr_rad = gyr * (np.pi / 180)
new_data.update_imu(gyr_rad, acc)
ahrs = new_data.quaternion.to_euler_angles()
# print ahrs
time.sleep(1)
# ------------------------------- # Initiate AHRS refq = Quaternion(1,0,0,0) # look front #refq = Quaternion(axis=[0,0,1],angle=np.pi/2) # look right #refq = Quaternion(axis=[1,0,0],angle=np.pi/2) # look front, roll 90' left-down #refq = Quaternion(axis=[0,1,0],angle=np.pi/2) # look up ahrs = MadgwickAHRS(sampleperiod=0.0005, beta=0.1, quaternion=refq) while Sensor or options.sim: if options.sim: # use fake data for testing #ahrs.update_imu((1, 0, 0), (0, 0, 1)) # sideways, pitching #ahrs.update_imu((0, 0, 1), (1, 0, 0)) # looking front, yawing to right ahrs.update_imu((0, 1, 0), (0, 1, 0)) # looking right, pitching up else: data = Sensor.read(64) if data == None: continue frame = sensor.parse(data[16:32]) time1 = frame["timestamp"] if time1 < time2: time2 = time2 - (1 << 24) if time2: delta = (time1 - time2) / 1000000 else: # Assume 500us for first sample delta = 500 / 1000000
plt.xlabel('Time (s)')
# -------------------------------------------------------------------------
# Compute orientation
from madgwickahrs import MadgwickAHRS
quat = [None] * len(time)
AHRSalgorithm = MadgwickAHRS(sampleperiod=1 / Fs)
# Initial convergence
initPeriod = tempo_parado # usually 2 seconds
indexSel = time < (tempo_parado + time[0])
for i in range(2000):
AHRSalgorithm.update_imu(
[0, 0, 0],
[accX[indexSel].mean(), accY[indexSel].mean(), accZ[indexSel].mean()])
# [magX[indexSel].mean(), magY[indexSel].mean(), magZ[indexSel].mean()])
# For all data
for t in range(len(time)):
if stationary[t]:
AHRSalgorithm.beta = 0.5
else:
AHRSalgorithm.beta = 0
AHRSalgorithm.update_imu(np.deg2rad([gyrX[t], gyrY[t], gyrZ[t]]),
[accX[t], accY[t], accZ[t]])
quat[t] = AHRSalgorithm.quaternion
quats = []
acc = []
gyro = []
while True:
#start = utime.ticks_ms()
accelerometer = sensor.accel
gyrometer = sensor.gyro
acc = umatrix.matrix([accelerometer.x, accelerometer.y, accelerometer.z],
cstride=1,
rstride=3,
dtype=float)
gyro = umatrix.matrix([gyrometer.x, gyrometer.y, gyrometer.z],
cstride=1,
rstride=3,
dtype=float)
#print("acceleration [X:%0.2f, Y:%0.2f, Z:%0.2f] gyro [X:%0.2f, Y:%0.2f, Z:%0.2f] \n"
# %(accelerometer.x, accelerometer.y, accelerometer.z, gyrometer.x * math.pi/180, gyrometer.y * math.pi/180, gyrometer.z * math.pi/180))
#print("temperature [T:%0.2f]\n" %(sensor.temperature))
ahrs.update_imu(gyro * math.pi / 180.0, acc)
(roll, pitch, yaw) = ahrs.quaternion.to_euler()
print("roll: %0.2f, pitch: %0.2f yaw: %0.2f\n" % (roll, pitch, yaw))
servo0.angle(math.pi / 2 - pitch)
servo1.angle(math.pi / 2 + yaw)
#print("Elapsed: %d" % (utime.ticks_ms() - start))
throw_ind = (np.where(throw_states == 1))[0][0]
except:
print("No throw detected")
a_values = np.vstack((ax_values_h, ay_values_h, az_values_h))
g_values = np.vstack((gx_values_h, gy_values_h, gz_values_h))
m_values = np.vstack((mx_values_h, my_values_h, mz_values_h))
# Use the Madgwick AHRS filter
ahrs = MadgwickAHRS()
R = np.zeros((m, 3, 3))
Z_flip = [[1, 0, 0], [0, 1, 0], [0, 0, -1]]
for i in range(m):
ahrs.update_imu(g_values[:, i], a_values[:, i])
#ahrs.update(g_values[:, i], a_values[:, i], m_values[:, i])
R[i, :, :] = Rotation.from_quat(ahrs.quaternion._q).as_matrix()
# Compute the tilt compensated acceleration and subtract gravity
tc_a_values = np.zeros((3, m))
for i in range(m):
tc_a_values[:, i] = np.dot(R[i, :, :], a_values[:, i])
a_values_final = tc_a_values - np.vstack(
(np.zeros(m), np.zeros(m), np.ones(m) * 9.81))
# Integrate acceleration to get velocity, then high pass filter
#start estimation loop
for i in range(start, len(imu_df)):
#get current imu point
imu_point = imu_df.iloc[i]
acceleration = imu_point[['accel_x', 'accel_y', 'accel_z'
]].values / 16384 * 9.8
omega = imu_point[['gyro_x', 'gyro_y', 'gyro_z'
]].values / 65535 * 2 * 250 * np.pi / 180
time = imu_point['timestamp_optitrack']
if time > 0:
if estimator.is_valid_time(time):
#madgwick attitude estimator
comp_filt.update_imu(omega, acceleration)
comp_quat = comp_filt.quaternion
comp_rot = Rotation(comp_quat[0], comp_quat[1], comp_quat[2],
comp_quat[3]) #sensor to earth
humcomp_rot = Rotation(1, 0, 0, 0).from_rotation_matrix(
np.linalg.inv(
comp_rot.to_rotation_matrix())) #earth to sensor
comp_eulers.append(comp_rot.to_euler_YPR())
rotated_grav = comp_rot.to_rotation_matrix() @ np.array(
[0, 0, 9.8]).reshape((3, 1))
#predict step
rot_idx = np.searchsorted(gnd_time, time)
if rot_idx < len(rotations):
rot_mat = rotations[rot_idx].to_rotation_matrix()
else:
