def populateSensorMap(robot, gps_service):
#pos = gps_service.getPosition()
valid = mpu9150.mpuRead()
if valid >= 0:
euler = mpu9150.getFusedEuler()
gyro = mpu9150.getRawGyro()
else:
euler = {0, 0, 0}
gyro = {0, 0, 0}
sensor_map = {}
sensor_map[Controller.imu_key] = (valid, euler, gyro)
sensor_map[Controller.gps_key] = gps_service.getPosition()
#TODO: add odometry and blobs
return sensor_map
def populateSensorMap(robot, gps_service):
#pos = gps_service.getPosition()
valid = mpu9150.mpuRead()
if valid >= 0:
euler = mpu9150.getFusedEuler()
gyro = mpu9150.getRawGyro()
else:
euler = {0, 0, 0}
gyro = {0, 0, 0}
sensor_map = {}
sensor_map[Controller.imu_key] = (valid, euler, gyro)
sensor_map[Controller.gps_key] = gps_service.getPosition()
#TODO: add odometry and blobs
return sensor_map
def getAttitude(self):
valid = mpu9150.mpuRead()
if valid >= 0:
euler = mpu9150.getFusedEuler()
# following compensate for magnetic declination, need to check if we need to substract or add declination value
corrected_heading = euler[2] - self.declination
if corrected_heading > 180.0:
corrected_heading = corrected_heading - 360.0
elif corrected_heading < -180.0:
corrected_heading = corrected_heading + 360.0
euler = (euler[0], euler[1], corrected_heading)
gyro = mpu9150.getRawGyro()
else:
euler = (0.0, 0.0, 0.0)
gyro = (0.0, 0.0, 0.0)
return (valid, euler, gyro)
def getAttitude(self):
valid = mpu9150.mpuRead()
if valid >= 0:
euler = mpu9150.getFusedEuler()
#following compensate for magnetic declination, need to check if we need to substract or add declination value
corrected_heading = euler[2] - self.declination
if corrected_heading > 180.0:
corrected_heading = (corrected_heading - 360.0)
elif corrected_heading < -180.0:
corrected_heading = (corrected_heading + 360.0)
euler = (euler[0], euler[1], corrected_heading)
gyro = mpu9150.getRawGyro()
else:
euler = (0.0, 0.0, 0.0)
gyro = (0.0, 0.0, 0.0)
return (valid, euler, gyro)
import mpu9150
import time
from math import *
mpu9150.mpuInit(1, 10, 4)
mpu9150.setMagCal('magcal.txt')
mpu9150.setAccCal('accelcal.txt')
while True:
i = mpu9150.mpuRead()
if i >= 0:
#print mpu9150.getFusedEuler()
mag = mpu9150.getCalMag()
bearing = atan2(mag[0], mag[1]) * (180.0 / pi)
if bearing < 0:
bearing = bearing + 360
print bearing
time.sleep(0.1)
def getPlatformAttitude(self): i = mpu9150.mpuRead() return (i, mpu9150.getFusedEuler(), mpu9150.getRawGyro())
def getPlatformAttitude(self): i = mpu9150.mpuRead() return (i, mpu9150.getFusedEuler(), mpu9150.getRawGyro())
import mpu9150
import time
from math import *
mpu9150.mpuInit(1, 10, 4)
mpu9150.setMagCal('magcal.txt')
mpu9150.setAccCal('accelcal.txt')
while True :
i = mpu9150.mpuRead()
if i >= 0:
#print mpu9150.getFusedEuler()
mag = mpu9150.getCalMag()
bearing = atan2(mag[0], mag[1])*(180.0/pi)
if bearing < 0 :
bearing = bearing +360
print bearing
time.sleep(0.1)
