コード例 #1
0
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
コード例 #2
0
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
コード例 #3
0
 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)
コード例 #4
0
 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)
コード例 #5
0
ファイル: test_dmp.py プロジェクト: yisea123/ic_fpga-logipi
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)
コード例 #6
0
	def getPlatformAttitude(self):
		i = mpu9150.mpuRead()
		return (i, mpu9150.getFusedEuler(), mpu9150.getRawGyro()) 
コード例 #7
0
	def getPlatformAttitude(self):
		i = mpu9150.mpuRead()
		return (i, mpu9150.getFusedEuler(), mpu9150.getRawGyro())
コード例 #8
0
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)