Python setMagCal示例

示例#1

def nav_loop():
    robot = UgvPlatform()
    #robot.setColorLut('lut_file.lut')
    #robot.initImu('accelcal.txt', 'magcal.txt')
    mpu_valid = -1
    print "waiting for GPS fix"
    gps_service = GpsService()
    current_pos = gps_service.getPosition()
    while not current_pos.valid:
        time.sleep(1)
        current_pos = gps_service.getPosition()
        print current_pos.valid
    while mpu_valid < 0:
        time.sleep(1.0)
        mpu_valid = mpu9150.mpuInit(1, 10, 4)
    print mpu_valid
    mpu9150.setMagCal('./magcal.txt')
    mpu9150.setAccCal('./accelcal.txt')
    #time.sleep(4)
    wp = StaticWayPointProvider()
    controller = Controller()
    threads.append(controller)
    #gps_service.start()
    robot.setSteeringFailSafe(0.0)
    robot.setSpeedFailSafe(ESC_ARM_ANGLE)
    robot.resetWatchdog()
    robot.setSteeringAngle(0.0)
    robot.setSpeed(0)
    while True:
        robot.resetWatchdog()
        target_pos = wp.getCurrentWayPoint()
        sensors = populateSensorMap(robot, gps_service)
        current_pos = sensors[Controller.gps_key]
        imu_sample = sensors[Controller.imu_key]
        if imu_sample[0] < 0:
            time.sleep(0.05)
            continue
        #print "lat : "+str(current_pos.lat)+" lon :"+str(current_pos.lon)+"imu:"+str(imu_sample)


#		continue
        cmd = controller.getCommand(sensors)
        if cmd == None:
            break
        print str(cmd)
        if cmd.has_key(Controller.next_waypoint_key
                       ) and cmd[Controller.next_waypoint_key] != None and cmd[
                           Controller.next_waypoint_key] == 1:
            wp.getNextWaypoint()
        else:
            robot.setSteeringAngle(cmd[Controller.steering_key])
            robot.setSpeed(cmd[Controller.speed_key])
        time.sleep(0.100)
        robot.setSteeringAngle(0.0)
    robot.setSpeed(0)
    print "Shutdown ESC then quit programm (for safety reason)"
    while True:
        time.sleep(1)

示例#2

文件： ugv_navigation.py 项目： JamesHyunKim/logi-projects

def nav_loop():
	robot = UgvPlatform()
	#robot.setColorLut('lut_file.lut')
	#robot.initImu('accelcal.txt', 'magcal.txt')
	mpu_valid = -1
	print "waiting for GPS fix"
	gps_service = GpsService()
	current_pos = gps_service.getPosition()
	while not current_pos.valid:
		time.sleep(1)
		current_pos = gps_service.getPosition()
		print current_pos.valid
	while mpu_valid < 0:
		time.sleep(1.0)
		mpu_valid = mpu9150.mpuInit(1, 10, 4)
	print mpu_valid
	mpu9150.setMagCal('./magcal.txt')
	mpu9150.setAccCal('./accelcal.txt')
	#time.sleep(4)
	wp = StaticWayPointProvider()
	controller = Controller()
	threads.append(controller)
	#gps_service.start()
	robot.setSteeringFailSafe(0.0)
	robot.setSpeedFailSafe(ESC_ARM_ANGLE)
	robot.resetWatchdog()
	robot.setSteeringAngle(0.0)
	robot.setSpeed(0)
	while True:
		robot.resetWatchdog()
		target_pos= wp.getCurrentWayPoint()
		sensors = populateSensorMap(robot, gps_service)
		current_pos = sensors[Controller.gps_key]
		imu_sample = sensors[Controller.imu_key]
		if imu_sample[0] < 0 :
			time.sleep(0.05)
			continue
		#print "lat : "+str(current_pos.lat)+" lon :"+str(current_pos.lon)+"imu:"+str(imu_sample)	
#		continue
		cmd = controller.getCommand(sensors)
		if cmd == None:
			break
		print str(cmd)
		if cmd.has_key(Controller.next_waypoint_key) and cmd[Controller.next_waypoint_key] != None and cmd[Controller.next_waypoint_key] == 1:
			wp.getNextWaypoint()
		else:	
			robot.setSteeringAngle(cmd[Controller.steering_key])
			robot.setSpeed(cmd[Controller.speed_key])
		time.sleep(0.100)
		robot.setSteeringAngle(0.0)        
	robot.setSpeed(0)
	print "Shutdown ESC then quit programm (for safety reason)"
        while True:
		time.sleep(1)	

示例#3

文件： 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)

示例#4

文件： avc_platform.py 项目： JamesHyunKim/logi-projects

	def initImu(self, acc_cal_file, mag_cal_file):
		mpu9150.mpuInit(1, 10, 4)
		mpu9150.setMagCal(mag_cal_file)
		mpu9150.setAccCal(acc_cal_file)

示例#5

文件： imu_service.py 项目： iUniverse/logi-projects

 def setCalibrationFiles(self, mag, acc):
     mpu9150.setMagCal(mag)
     mpu9150.setAccCal(acc)

示例#6

	def initImu(self, acc_cal_file, mag_cal_file):
		mpu9150.mpuInit(1, 10, 4)
		mpu9150.setMagCal(mag_cal_file)
		mpu9150.setAccCal(acc_cal_file)

示例#7

文件： test_dmp.py 项目： JamesHyunKim/logi-projects

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)

示例#8

文件： imu_service.py 项目： yisea123/ic_fpga-logipi

 def setCalibrationFiles(self, mag, acc):
     mpu9150.setMagCal(mag)
     mpu9150.setAccCal(acc)