数据采集代码
源码的数据采集程序,可见第38行其中使用了pollData和update进行数据采集。
void LpmsSensorManager::run(void)
{
MicroMeasure mm;
float prevTimestamp = 0.0f;
int deviceType = 0;
#ifdef _WIN32
ce.connect();
// be.connect();
#endif
#ifdef ANDROID
LOGV("[LpmsSensorManager] Thread running\n");
#endif
mm.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 _WIN32
ce.poll();
#endif
lm.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 _WIN32
ce.listDevices(&deviceList);
// be.listDevices(&deviceList);
#endif
if (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_U
if (managerState != SMANAGER_LIST)
break;
LpmsU::listDevices(&deviceList);
#endif
#ifdef _WIN32
if (managerState != SMANAGER_LIST)
break;
LpmsU2::listDevices(&deviceList);
#endif
#ifdef BUILD_BLUETOOTH
if (managerState != SMANAGER_LIST)
break;
LpmsBBluetooth::listDevices(&deviceList);
#endif
managerState = 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_PRIORITY
HANDLE th = t.native_handle();
SetThreadPriority(th, THREAD_PRIORITY_HIGHEST);
#endif
#endif
t.detach();
#ifdef ANDROID
LOGV("[LpmsSensorManager] Started");
#endif
}
可见就是不断执行update进行数据的采集,update程序如下:
... ...
// Main measurement state
case STATE_MEASURE:
assertConnected();
// Start next measurement step only if program is not waiting for data or ACK
if (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 angle
if (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 measurement
if ((bt->getConfigReg() & LPMS_MAG_RAW_OUTPUT_ENABLED) != 0) {
vectSub3x1(&bRaw, &configData.hardIronOffset, &b);
matVectMult3(&configData.softIronMatrix, &b, &b);
}
else {
vectZero3x1(&b);
}
// Corrects accelerometer measurement
if ((bt->getConfigReg() & LPMS_ACC_RAW_OUTPUT_ENABLED) != 0) {
matVectMult3(&configData.misalignMatrix, &aRaw, &a);
vectAdd3x1(&configData.accBias, &a, &a);
}
else {
vectZero3x1(&a);
}
// Corrects gyro measurement
if ((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 active
checkMagCal(frameTime);
checkPlanarMagCal(frameTime);
checkMisalignCal(frameTime);
checkGyrMisalignCal(frameTime);
checkMagMisalignCal(frameTime);
checkMagReferenceCal(frameTime);
// Sets current datac
setCurrentData(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 active
checkSaveData(); //检测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);
}
PPT演示调用
Tcp通讯时序详解
首先客户端主动发起连接、发送请求,然后服务器端响应请求,然后客户端主动关闭连接。两条竖线表示通讯的两端,从上到下表示时间的先后顺序,注意,数据从一端传到网络的另一端也需要时间,所以图中的箭头都是斜的。双方发送的段按时间顺序编号为1-10,各段中的主要信息在箭头上标出,例如段2的箭头上标着SYN, 8000(0), ACK1001, ,表示该段中的SYN位置1,32位序号是8000,该段不携带有效载荷(数据字节数为0),ACK位置1,32位确认序号是1001,带有一个mss(Maximum Segment Size,最大报文长度)选项值为1024。
建立连接(三次握手)的过程:
- 客户端发送一个带SYN标志的TCP报文到服务器。这是三次握手过程中的段1。
客户端发出段1,SYN位表示连接请求。序号是1000,这个序号在网络通讯中用作临时的地址,每发一个数据字节,这个序号要加1,这样在接收端可以根据序号排出数据包的正确顺序,也可以发现丢包的情况,另外,规定SYN位和FIN位也要占一个序号,这次虽然没发数据,但是由于发了SYN位,因此下次再发送应该用序号1001。mss表示最大段尺寸,如果一个段太大,封装成帧后超过了链路层的最大帧长度,就必须在IP层分片,为了避免这种情况,客户端声明自己的最大段尺寸,建议服务器端发来的段不要超过这个长度。
- 服务器端回应客户端,是三次握手中的第2个报文段,同时带ACK标志和SYN标志。它表示对刚才客户端SYN的回应;同时又发送SYN给客户端,询问客户端是否准备好进行数据通讯。
服务器发出段2,也带有SYN位,同时置ACK位表示确认,确认序号是1001,表示“我接收到序号1000及其以前所有的段,请你下次发送序号为1001的段”,也就是应答了客户端的连接请求,同时也给客户端发出一个连接请求,同时声明最大尺寸为1024。
- 客户必须再次回应服务器端一个ACK报文,这是报文段3。
客户端发出段3,对服务器的连接请求进行应答,确认序号是8001。在这个过程中,客户端和服务器分别给对方发了连接请求,也应答了对方的连接请求,其中服务器的请求和应答在一个段中发出,因此一共有三个段用于建立连接,称为“三方握手(three-way-handshake)”。在建立连接的同时,双方协商了一些信息,例如双方发送序号的初始值、最大段尺寸等。
在TCP通讯中,如果一方收到另一方发来的段,读出其中的目的端口号,发现本机并没有任何进程使用这个端口,就会应答一个包含RST位的段给另一方。例如,服务器并没有任何进程使用8080端口,我们却用telnet客户端去连接它,服务器收到客户端发来的SYN段就会应答一个RST段,客户端的telnet程序收到RST段后报告错误Connection refused:
$ telnet 192.168.0.200 8080
Trying 192.168.0.200...
telnet: Unable to connect to remote host: Connection refused
数据传输的过程:
客户端发出段4,包含从序号1001开始的20个字节数据。
服务器发出段5,确认序号为1021,对序号为1001-1020的数据表示确认收到,同时请求发送序号1021开始的数据,服务器在应答的同时也向客户端发送从序号8001开始的10个字节数据,这称为piggyback。
客户端发出段6,对服务器发来的序号为8001-8010的数据表示确认收到,请求发送序号8011开始的数据。
在数据传输过程中,ACK和确认序号是非常重要的,应用程序交给TCP协议发送的数据会暂存在TCP层的发送缓冲区中,发出数据包给对方之后,只有收到对方应答的ACK段才知道该数据包确实发到了对方,可以从发送缓冲区中释放掉了,如果因为网络故障丢失了数据包或者丢失了对方发回的ACK段,经过等待超时后TCP协议自动将发送缓冲区中的数据包重发。
关闭连接(四次握手)的过程:
由于TCP连接是全双工的,因此每个方向都必须单独进行关闭。这原则是当一方完成它的数据发送任务后就能发送一个FIN来终止这个方向的连接。收到一个FIN只意味着这一方向上没有数据流动,一个TCP连接在收到一个FIN后仍能发送数据。首先进行关闭的一方将执行主动关闭,而另一方执行被动关闭。
客户端发出段7,FIN位表示关闭连接的请求。
服务器发出段8,应答客户端的关闭连接请求。
服务器发出段9,其中也包含FIN位,向客户端发送关闭连接请求。
客户端发出段10,应答服务器的关闭连接请求。
建立连接的过程进行三次握手连接,而关闭连接通常需要4个段,服务器的应答和关闭连接请求通常不合并在一个段中,因为有连接半关闭的情况,这种情况下客户端关闭连接之后就不能再发送数据给服务器了,但是服务器还可以发送数据给客户端,直到服务器也关闭连接为止。所以由于Sever端发送确认信息和数据信息只用了一次握手,所以比断开少一次,TCP连接是全双工的,因此每个方向都必须单独进行关闭,所以进行了四次握手断开
根据上述所讲写出Cpp与Python程序的通讯的握手时序图