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)