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)
def getPlatformAttitude(self): i = mpu9150.mpuRead() return (i, mpu9150.getFusedEuler(), mpu9150.getRawGyro())
def getPlatformAttitude(self): i = mpu9150.mpuRead() return (i, mpu9150.getFusedEuler(), mpu9150.getRawGyro())
