LPMS-B2 数据采集源码分析
本文是针对 LPMS-B2 数据采集源码进行分析, LPMS-B2 是 LP公司出品的一款IMU,最近用到特别总结。
源码的数据采集程序,可见第38行其中使用了pollData和update进行数据采集。
void LpmsSensorManager::run(void)
{MicroMeasure mm;float prevTimestamp = 0.0f;int deviceType = 0;#ifdef _WIN32 ce.connect();// be.connect();
#endif #ifdef ANDROIDLOGV("[LpmsSensorManager] Thread running\n");
#endifmm.reset();int sleepFlag = 0;while (stopped == false) {switch (managerState) {case SMANAGER_MEASURE:lm.lock();for (auto i = sensorList.begin(); i != sensorList.end(); i++) {(*i)->pollData();//数据采集}#ifdef _WIN32ce.poll();
#endiflm.unlock();if (mm.measure() > threadDelay) {mm.reset();lm.lock();for (auto i = sensorList.begin(); i != sensorList.end(); i++) {(*i)->update(); //数据采集}lm.unlock();} else {std::this_thread::sleep_for(std::chrono::microseconds(100));}break;case SMANAGER_LIST:deviceList.clear();#ifdef _WIN32ce.listDevices(&deviceList);// be.listDevices(&deviceList);
#endifif (managerState != SMANAGER_LIST)break;if (scan_serial_ports_ == true){if (verbose)logd(TAG, "List RS2323 devices\n");LpmsRS232::listDevices(&deviceList);}// if (managerState != SMANAGER_LIST)// break;// LpmsTcp::listDevices(&deviceList);#ifdef BUILD_LPMS_Uif (managerState != SMANAGER_LIST)break;LpmsU::listDevices(&deviceList);
#endif
#ifdef _WIN32if (managerState != SMANAGER_LIST)break;LpmsU2::listDevices(&deviceList);
#endif
#ifdef BUILD_BLUETOOTHif (managerState != SMANAGER_LIST)break;LpmsBBluetooth::listDevices(&deviceList);
#endifmanagerState = SMANAGER_MEASURE;break;}#ifdef __GNUC__std::this_thread::sleep_for(std::chrono::milliseconds(1));
#endif}#ifdef _WIN32 ce.close();// be.close();
#endif
}
在其声明的时候就new了一个线程进行run操作
LpmsSensorManager::LpmsSensorManager(JavaVM *thisVm, jobject bluetoothAdapter) :
thisVm(thisVm),
bluetoothAdapter(bluetoothAdapter)
#endif
{stopped = false;isRecording = false;threadDelay = 500;currentUartBaudrate = SELECT_LPMS_UART_BAUDRATE_115200;verbose = true;managerState = SMANAGER_MEASURE;std::thread t(&LpmsSensorManager::run, this); //新建线程,执行run函数#ifdef _WIN32
#ifdef THREAD_HIGH_PRIORITYHANDLE th = t.native_handle();SetThreadPriority(th, THREAD_PRIORITY_HIGHEST);
#endif
#endift.detach();
#ifdef ANDROIDLOGV("[LpmsSensorManager] Started");
#endif
}
可见就是不断执行update进行数据的采集,update程序如下:
... ...
// Main measurement statecase STATE_MEASURE:assertConnected();// Start next measurement step only if program is not waiting for data or ACKif (bt->isWaitForData() == false && bt->isWaitForAck() == false) {if (bt->getMode() != SELECT_LPMS_MODE_STREAM) {bt->setStreamMode();prepareStream = 0;break;}}// TODO: Insert error handling for sensor.// if (bt->isError() == true) {// setSensorStatus(SENSOR_STATUS_ERROR);// }if (paused == true) {break;}if (prepareStream < STREAM_N_PREPARE) {++prepareStream;break;}// Load current data from hardware and calculate rotation matrix and Euler angleif (bt->getLatestImuData(&imuData) == false) break; //可见是从imuDataQueue弹出imuData
/*
bool LpmsIoInterface::getLatestImuData(ImuData *id)
{if (imuDataQueue.empty() == true) return false;*id = imuDataQueue.front();imuDataQueue.pop();return true;
}
*/frameTime = lpmsTimer.measure() / 1000.0f;lpmsTimer.reset();setFps(frameTime);convertArrayToLpVector4f(imuData.q, &q);quaternionToMatrix(&q, &m);convertLpMatrixToArray(&m, imuData.rotationM);// Add frame number timestamp and IMU ID to current ImuData++frameNo;imuData.frameCount = frameNo;imuData.openMatId = configData.openMatId;setConnectionStatus(SENSOR_CONNECTION_CONNECTED);if (isMagCalibrationEnabled == true) {setSensorStatus(SENSOR_STATUS_CALIBRATING);}else {if (paused == false) {setSensorStatus(SENSOR_STATUS_RUNNING);}else {setSensorStatus(SENSOR_STATUS_PAUSED);}}convertArrayToLpVector3f(imuData.aRaw, &aRaw);convertArrayToLpVector3f(imuData.bRaw, &bRaw);convertArrayToLpVector3f(imuData.gRaw, &gRaw);// Corrects magnetometer measurementif ((bt->getConfigReg() & LPMS_MAG_RAW_OUTPUT_ENABLED) != 0) {vectSub3x1(&bRaw, &configData.hardIronOffset, &b);matVectMult3(&configData.softIronMatrix, &b, &b);}else {vectZero3x1(&b);}// Corrects accelerometer measurementif ((bt->getConfigReg() & LPMS_ACC_RAW_OUTPUT_ENABLED) != 0) {matVectMult3(&configData.misalignMatrix, &aRaw, &a);vectAdd3x1(&configData.accBias, &a, &a);}else {vectZero3x1(&a);}// Corrects gyro measurementif ((bt->getConfigReg() & LPMS_GYR_RAW_OUTPUT_ENABLED) != 0) {matVectMult3(&configData.gyrMisalignMatrix, &gRaw, &g);vectAdd3x1(&configData.gyrAlignmentBias, &g, &g);}else {vectZero3x1(&g);}convertLpVector3fToArray(&a, imuData.a);convertLpVector3fToArray(&b, imuData.b);convertLpVector3fToArray(&g, imuData.g);// Checks, if calibration is activecheckMagCal(frameTime);checkPlanarMagCal(frameTime);checkMisalignCal(frameTime);checkGyrMisalignCal(frameTime);checkMagMisalignCal(frameTime);checkMagReferenceCal(frameTime);// Sets current datacsetCurrentData(imuData); //可见其实现是将数据压到dataQueue,当其长度小于dataQueueLength时。
/*
void LpmsSensor::setCurrentData(ImuData d)
{std::unique_lock<std::mutex> lock(sensorMutex);currentData = d;if (dataQueue.size() < dataQueueLength) {dataQueue.push(d);}else {dataQueue.pop();dataQueue.push(d);}if (lpmsCallback) {lpmsCallback(d, deviceId.c_str());}newDataCondition.notify_one();
}
*/// Checks, if data saving is activecheckSaveData(); //检测save与否,并执行操作break;
... ...
该程序中值得注意的有两个函数,一个函数是getLatestImuData 可见是从imuDataQueue弹出imuData。
bool LpmsIoInterface::getLatestImuData(ImuData *id)
{if (imuDataQueue.empty() == true) return false;*id = imuDataQueue.front();imuDataQueue.pop();return true;
}
一个函数是setCurrentData,可见其实现是将数据压到dataQueue,当其长度小于dataQueueLength时。
void LpmsSensor::setCurrentData(ImuData d)
{std::unique_lock<std::mutex> lock(sensorMutex);currentData = d;if (dataQueue.size() < dataQueueLength) {dataQueue.push(d);}else {dataQueue.pop();dataQueue.push(d);}if (lpmsCallback) {lpmsCallback(d, deviceId.c_str());}newDataCondition.notify_one();
}
然后查看我们使用的getCurrentData函数,其是从dataQueue弹出的数据,也就是说不需要跟传感器通信,我们只需要从dataQueue中获取数据即可,但是应该保证数据采集程序在数据采集周期将数据取出,如果不行的话,则会导致数据丢失,即自编上位机时不需要多线程进行数据采集,只使用while循环就可以完成数据采集,多线程反而导致电脑性能不足而导致数据丢失。
ImuData LpmsSensor::getCurrentData(void)
{ImuData d;bt->zeroImuData(&d);sensorMutex.lock();if (dataQueue.size() > 0) {d = dataQueue.front();dataQueue.pop();}else {d = currentData;}sensorMutex.unlock();return d;
}
对于imuDataQueue的获得是在蓝牙程序parseSensorData中实现的。
bool LpmsBle::parseSensorData(void)
{unsigned o=0;const float r2d = 57.2958f;int iTimestamp;int iQuat;int iHeave;zeroImuData(&imuData); fromBufferInt16(oneTx, o, &iTimestamp);o = o + 2;currentTimestamp = (float) iTimestamp;if (timestampOffset > currentTimestamp) timestampOffset = currentTimestamp;imuData.timeStamp = currentTimestamp - timestampOffset;fromBufferInt16(oneTx, o, &iQuat);o = o + 2;imuData.q[0] = (float) iQuat / (float) 0x7fff;fromBufferInt16(oneTx, o, &iQuat);o = o + 2;imuData.q[1] = (float) iQuat / (float) 0x7fff;fromBufferInt16(oneTx, o, &iQuat);o = o + 2;imuData.q[2] = (float) iQuat / (float) 0x7fff;fromBufferInt16(oneTx, o, &iQuat);o = o + 2;imuData.q[3] = (float) iQuat / (float) 0x7fff;fromBufferInt16(oneTx, o, &iHeave);o = o + 2;imuData.hm.yHeave = (float) iHeave / (float) 0x0fff;if (imuDataQueue.size() < 64) {imuDataQueue.push(imuData);}return true;
}
其中parseSensorData在parseFunction中调用。
bool LpmsIoInterface::parseFunction(void)
{... ...case GET_SENSOR_DATA:parseSensorData();break;... ...
}
parseFunction在函数parseModbusByte中调用。
bool LpmsBBluetooth::parseModbusByte(void){ ... ...case PACKET_LRC_CHECK1:lrcReceived = lrcReceived + ((unsigned)b * 256);if (lrcReceived == lrcCheck) {parseFunction();}else {if (verbose) logd(TAG, "Checksum fail in data packet\n");}rxState = PACKET_END;break;... ...
}
parseModbusByte在checkState中调用。
bool LpemgIoInterface::checkState(void)
{parseModbusByte();... ...
}
checkState在pollData中调用。
void LpmsSensor::pollData(void)
{if (bt->deviceStarted() == true) {if (!bt->pollData())if (verbose) logd(TAG, "Poll Data error: %s\n", bt->getErrorMsg().c_str());bt->checkState();}
}
可见pollData实现了从传感器获取数据,保存至imuDataQueue,而update实现了数据处理并将数据保存至dataQueue。
下面是数据采集的子线程。
bool IMUDAQ_Task::IMUDAQ()
{bool first = true;timeb start, end;int i[4] = { 0,0,0,0};int j = 0;while (1) {ftime(&start);ftime(&end);while ((end.millitm - start.millitm + 1000 * (end.time - start.time) <= period * 1000 || stopbyuser || onceonly)&& running){j = 0;for (auto lpms : Lpms) {if (lpms->hasImuData() > 0) {imudata = lpms->getCurrentData();memcpy(Quaternion.data, imudata.q, 4 * sizeof(float));scalarVectMult4x1(vect4x1Norm(Quaternion), &Quaternion, &QuaternionNormal);memcpy(LinAcc.data, imudata.linAcc, 3 * sizeof(float));quatRotVec(QuaternionNormal, LinAcc, &GlobalLinAcc);if (!send) {leg = Leg[j / 2];legposition = Legposition[j % 2];}else {signal.set_leg(ImuTutorial::Signal::Leg(j / 2));signal.set_legposition(ImuTutorial::Signal::LegPosition(j % 2));}savedata();i[j]++;}j++;}if (first){first = false;if (onceonly)break;}ftime(&end);}if(end.millitm - start.millitm + 1000 * (end.time - start.time) > period*1000 || stopbyuser || onceonly)processing = false;if ((running == false)&&!first){break;}if ((running == true) && ((processing == false)|| onceonly)) {running = false;break;}}std::cout << std::endl;for (int n = 0; n < 4; n++)std::cout << Leg[n / 2] << " " << Legposition[n % 2] << " data lenght:" << i[n] << std::endl;if (processing == false && !send) IMU_log.close();t = nullptr;for (auto lpms:Lpms)lpms->pause();std::cout << "Data Acquisition over !" << std::endl;std::cout << "Please enter your command : ";return(true);
}
流程图展示
LPMS-B2 数据采集源码分析相关推荐
- 极光实时监听怎么调用_源码分析 Sentinel 实时数据采集实现原理(图文并茂)
本篇将重点关注 Sentienl 实时数据收集,即 Sentienl 具体是如何收集调用信息,以此来判断是否需要触发限流或熔断. Sentienl 实时数据收集的入口类为 StatisticSlot. ...
- 源码分析 Sentinel 实时数据采集实现原理
本篇将重点关注 Sentienl 实时数据收集,即 Sentienl 具体是如何收集调用信息,以此来判断是否需要触发限流或熔断. 本节目录 1.源码分析 StatisticSlot 1.1 Stati ...
- Tomcat7.0源码分析——Session管理分析(上)
版权声明:本文为博主原创文章,未经博主允许不得转载. https://blog.csdn.net/beliefer/article/details/52450268 前言 对于广大java开发者而言, ...
- dubbo源码分析系列(1)扩展机制的实现
1 系列目录 dubbo源码分析系列(1)扩展机制的实现 dubbo源码分析系列(2)服务的发布 dubbo源码分析系列(3)服务的引用 dubbo源码分析系列(4)dubbo通信设计 2 SPI扩展 ...
- sigslot库源码分析
言归正传,sigslot是一个用标准C++语法实现的信号与槽机制的函数库,类型和线程安全.提到信号与槽机制,恐怕最容易想到的就是大名鼎鼎的Qt所支持的对象之间通信的模式吧.不过这里的信号与槽虽然在概念 ...
- docker push 过程 distribution源码 分析
docker push 过程 distribution源码分析 承接上一篇"distribution structure and start up 分析"本文分析一下distrib ...
- MyBatis学习笔记-源码分析篇
引言 SQL 语句的执行涉及多个组件,其中比较重要的是 Executor. StatementHandler. ParameterHandler 和 ResultSetHandler. Executo ...
- FFmpeg简述,源码分析,录制/压缩/水印/剪切/旋转/滤镜/美颜/上传视频等(CPU软编码和解码)
> ffmpeg源码分析 ffmpeg源码简析(一)结构总览- https://blog.csdn.net/Louis_815/article/details/79621056 FFmpeg的库 ...
- Skywalking源码分析【agent探针篇】
Skywalking agent源码分析 字节码技术 入口方法 1.核心配置加载方式: 2.插件初始化: 3.插件(中间件or框架)的增强 增强点的寻找: 4.服务启动 5.插件体系 5.1.拦截实例 ...
最新文章
- 用户信息检索另一台服务器,客户机上一用户访问另一台机器上的informix数据库...
- 【机器学习】集成学习与模型融合方法举例
- Linux LB 集群知识、如何用 LVS 方式实现 LB 集群?
- python简单好看的代码_Python新手写出漂亮的爬虫代码1
- springboot接收多对象_SpringBoot同时接收单个对象和List<object>参数
- K8S 基于NFS实现文件集群间共享
- python 字典操作
- VHDL----基础知识1
- 汇编语言中xor指令_汇编语言XOR指令:对两个操作数进行逻辑(按位)异或操作...
- 织梦5.7生成HTML很慢,Dedecms 生成静态网页速度特别慢的问题
- 计算机二级数据库系统知识点,2020年全国计算机二级Access复习知识点:分布式数据库系统...
- 具有分散效果的瀑布流
- Ardunio开发实例-BME280传感器
- 天下数据解析域名及域名转向
- python RGB与LCH互转 算法
- [转]ZBrush3官方中文教程 一
- Java Excel文件内容替换
- java wtc_java通过wtc调用tuxedo服务超时
- 解决Attribute url invalid for tag page according to TLD的问题
- EMD经验模态分解实例(转C代码)