def vn_200_imu_callback(data):
# read compass data
global X_OFFSET, Y_OFFSET, Z_OFFSET
xMag = data.compass.x - X_OFFSET
yMag = data.compass.y - Y_OFFSET
zMag = data.compass.z - Z_OFFSET
# mag low-pass filter
global alphaMag, xMagOld, yMagOld, zMagOld
xMag = xMag * alphaMag + xMagOld * (1 - alphaMag)
yMag = yMag * alphaMag + yMagOld * (1 - alphaMag)
zMag = zMag * alphaMag + zMagOld * (1 - alphaMag)
xMagOld = xMag
yMagOld = yMag
zMagOld = zMag
# normalize the mag values
norm = math.sqrt(xMag ** 2 + yMag ** 2 + zMag ** 2)
xMag /= norm
yMag /= norm
zMag /= norm
# read accel data
xAccel = data.accelerometer.x
yAccel = data.accelerometer.y
zAccel = data.accelerometer.z
# accel low-pass filter
global alphaAccel, xAccelOld, yAccelOld, zAccelOld
xAccel = xAccel * alphaAccel + xAccelOld * (1 - alphaAccel)
yAccel = yAccel * alphaAccel + yAccelOld * (1 - alphaAccel)
zAccel = zAccel * alphaAccel + zAccelOld * (1 - alphaAccel)
xAccelOld = xAccel
yAccelOld = yAccel
zAccelOld = zAccel
# normalize the mag values
norm = math.sqrt(xAccel ** 2 + yAccel ** 2 + zAccel ** 2)
xAccel /= norm
yAccel /= norm
zAccel /= norm
# calculate roll & pitch, then yaw
# src: http://arduino.cc/forum/index.php/topic,8573.0.html
roll = math.atan2(yAccel, math.sqrt(xAccel ** 2 + zAccel ** 2))
pitch = math.atan2(xAccel, math.sqrt(yAccel ** 2 + zAccel ** 2))
yaw = math.atan2(
-yMag * math.cos(roll) + zMag * math.sin(roll),
xMag * math.cos(pitch) + zMag * math.sin(pitch) * math.sin(roll) + zMag * math.sin(pitch) * math.cos(roll),
)
msg = Orientation()
msg.roll = bound0to2Pi(roll)
msg.pitch = bound0to2Pi(pitch)
msg.yaw = bound0to2Pi(math.pi * 2 - yaw + YAW_CORRECTION)
global pub
pub.publish(msg)
def vn_200_ins_callback(data):
roll = bound0to2Pi(data.orientation_euler.roll*math.pi/180.0)
pitch = bound0to2Pi(data.orientation_euler.pitch*math.pi/180.0)
yaw = -data.orientation_euler.yaw*math.pi/180.0
yaw = bound0to2Pi(yaw + YAW_CORRECTION)
yaw = math.pi*2 - yaw
msg = Orientation()
msg.roll = roll
msg.pitch = pitch
msg.yaw = yaw
global pub
pub.publish(msg)
def vn_200_ins_callback(data):
roll = bound0to2Pi(data.orientation_euler.roll * math.pi / 180.0)
pitch = bound0to2Pi(data.orientation_euler.pitch * math.pi / 180.0)
yaw = -data.orientation_euler.yaw * math.pi / 180.0
yaw = bound0to2Pi(yaw + YAW_CORRECTION)
yaw = math.pi * 2 - yaw
msg = Orientation()
msg.roll = roll
msg.pitch = pitch
msg.yaw = yaw
global pub
pub.publish(msg)
def vn_200_imu_callback(data):
# read compass data
global X_OFFSET, Y_OFFSET, Z_OFFSET
xMag = data.compass.x - X_OFFSET
yMag = data.compass.y - Y_OFFSET
zMag = data.compass.z - Z_OFFSET
# mag low-pass filter
global alphaMag, xMagOld, yMagOld, zMagOld
xMag = xMag*alphaMag + xMagOld*(1 - alphaMag)
yMag = yMag*alphaMag + yMagOld*(1 - alphaMag)
zMag = zMag*alphaMag + zMagOld*(1 - alphaMag)
xMagOld = xMag
yMagOld = yMag
zMagOld = zMag
# normalize the mag values
norm = math.sqrt(xMag**2 + yMag**2 + zMag**2)
xMag /= norm
yMag /= norm
zMag /= norm
# read accel data
xAccel = data.accelerometer.x
yAccel = data.accelerometer.y
zAccel = data.accelerometer.z
# accel low-pass filter
global alphaAccel, xAccelOld, yAccelOld, zAccelOld
xAccel = xAccel*alphaAccel + xAccelOld*(1 - alphaAccel)
yAccel = yAccel*alphaAccel + yAccelOld*(1 - alphaAccel)
zAccel = zAccel*alphaAccel + zAccelOld*(1 - alphaAccel)
xAccelOld = xAccel
yAccelOld = yAccel
zAccelOld = zAccel
# normalize the mag values
norm = math.sqrt(xAccel**2 + yAccel**2 + zAccel**2)
xAccel /= norm
yAccel /= norm
zAccel /= norm
# calculate roll & pitch, then yaw
# src: http://arduino.cc/forum/index.php/topic,8573.0.html
roll = math.atan2(yAccel, math.sqrt(xAccel**2 + zAccel**2))
pitch = math.atan2(xAccel, math.sqrt(yAccel**2 + zAccel**2))
yaw = math.atan2(
-yMag*math.cos(roll) + zMag*math.sin(roll),
xMag*math.cos(pitch) + zMag*math.sin(pitch)*math.sin(roll)
+ zMag*math.sin(pitch)*math.cos(roll)
)
msg = Orientation()
msg.roll = bound0to2Pi(roll)
msg.pitch = bound0to2Pi(pitch)
msg.yaw = bound0to2Pi(math.pi*2 - yaw + YAW_CORRECTION)
global pub
pub.publish(msg)
