def initDevice(self, imu_port, loglevel):
self.rgbd = RGBD_Camera(loglevel)
try:
self.rgbd.open()
except Exception as e:
self.log.error("RGBD相机打开失败: %r" % e)
return
ser = serial.Serial(imu_port, 115200)
ser.write('hello\r\n'.encode())
ser.close()
self.imu = LpmsME.LpmsME(imu_port, 115200)
if (not self.imu.connect()):
self.log.error("imu sensor连接失败!")
return
#self.param_960p = self.rgbd.get_color_params()[0]
#self.param_480p = self.rgbd.get_color_params()[1]
self.rgbd.create_streams()
#if(self.en_960p):
# self.rgbd.set_color_960p()
self.rgbd.set_dcsync()
self.rgbd.select_ImageRegistration(1)
self.rgbd.get_depth_value_unit()
# start preview
self.previewTimer = QTimer(self)
self.previewTimer.timeout.connect(self.preview)
self.enterPreviewState()
self.btn_cap.setEnabled(True)
def __init__(self, inputport):
'''
todo: 初始化一些配置 创建类后自动会执行这个_init_
'''
self.port = inputport
self.baudrate = 115200
self.lpmsSensor = LpmsME.LpmsME(self.port, self.baudrate)
self.lpmsSensor.connect()
self.spin()
def __init__(self):
self.imu_publisher = rospy.Publisher('super_minitaur/imu_data', ImuMsg, queue_size=10)
self.imu_clinet = rospy.ServiceProxy("/state_flag_imu",StateFlag)
self.lpmsSensor = LpmsME.LpmsME(IMU_PORT, IMU_BAUDRATE)
self.disconnect()
self.lpmsSensor.connect()
self.freq = IMU_FREQENCY
self.euler = ['', '', ''] # row pitch yaw
self.pitch_deque = deque(maxlen = 200)
self.old_flag = 0
])
stream_freq_menu = OrderedDict([
('0', set_stream_freq_5Hz),
('1', set_stream_freq_10Hz),
('2', set_stream_freq_25Hz),
('3', set_stream_freq_50Hz),
('4', set_stream_freq_100Hz),
('5', set_stream_freq_200Hz),
('6', set_stream_freq_400Hz),
])
port = 'COM5'
baudrate = 115200
lpmsSensor = LpmsME.LpmsME(port, baudrate)
quit = False
def main():
global quit
while not quit:
print_main_menu()
choice = raw_input(" >> ")
exec_menu(main_menu, choice)
disconnect_sensor()
lputils.logd(TAG, "bye")
if __name__ == "__main__":
from lpmslib import LpmsME
import time
imu = LpmsME.LpmsME('/dev/ttyAMA0', 115200)
if (not imu.connect()):
print("imu sensor连接失败!")
exit(0)
cnt = 0
while (True):
imu_data = imu.get_stream_data()[8]
print(imu_data)
time.sleep(0.1)
cnt += 1
if (cnt > 1000):
break
imu.disconnect()