Код: Выделить всё
GDBgdb info:
Код: Выделить всё
Thread 11 "ReadIMU" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fe92dd540 (LWP 3744)]
0x0000007ff5d41998 in __GI___clock_nanosleep (clock_id=, clock_id@entry=0, flags=flags@entry=0, req=0x7fe92dc640, rem=0x7fe92dc640) at ../sysdeps/unix/sysv/linux/clock_nanosleep.c:78
78 ../sysdeps/unix/sysv/linux/clock_nanosleep.c: No such file or directory.
(gdb)
(gdb)
(gdb)
(gdb)
(gdb) bt
#0 0x0000007ff5d41998 in __GI___clock_nanosleep (clock_id=, clock_id@entry=0, flags=flags@entry=0, req=0x7fe92dc640, rem=0x7fe92dc640) at ../sysdeps/unix/sysv/linux/clock_nanosleep.c:78
#1 0x0000007ff5d46b6c in __GI___nanosleep (req=, rem=) at ../sysdeps/unix/sysv/linux/nanosleep.c:25
#2 0x0000005555804e0c in std::this_thread::sleep_for (__rtime=...) at /usr/include/c++/11/bits/this_thread_sleep.h:82
#3 0x000000555598831c in mh4::host::controller::imu::IMUHandler::ReadIMU () at /MH4/src/host/controller/imu/ImuHandler.cpp:208
#4 0x00000055558a6adc in std::__invoke_impl (__f=) at /usr/include/c++/11/bits/invoke.h:61
#5 0x0001233b00026b37 in ?? ()
Backtrace stopped: previous frame identical to this frame (corrupt stack?)
code:
Код: Выделить всё
void IMUHandler::ReadIMU(void) {
char buffer[RX_BUF_LEN];
int nread = 0;
unsigned short cnt = 0;
int pos;
unsigned char recvBuf[RX_BUF_LEN] = {0};
unsigned short recvBufIdx = 0;
protocol_info_t outputInfo = {};
memset(buffer, 0, sizeof(buffer));
LOG("start to read imu.");
IMUHandler::getInstance().OpenIMU();
IMUHandler::getInstance().ConfigureIMU();
output_data_header_t header{};
unsigned int frame_len = 0;
while (1) {
try {
nread = read(IMUHandler::getInstance().m_serialFd, buffer, RX_BUF_LEN);
if (nread > 0) {
// printf("nread = %d\n", nread);
memcpy(recvBuf + recvBufIdx, buffer, nread);
recvBufIdx += nread;
}
cnt = recvBufIdx;
pos = 0;
if (cnt < YIS_OUTPUT_MIN_BYTES) {
usleep(1500);
continue;
}
while (cnt > (unsigned short)0) {
int ret = analysis_data(recvBuf + pos, cnt, &outputInfo);
if (analysis_done == ret) { /*未查找到帧头*/
pos++;
cnt--;
} else if (data_len_err == ret) {
break;
} else if (crc_err == ret ||
analysis_ok == ret) { /*删除已解析完的完整一帧*/
memcpy(&header, recvBuf + pos, sizeof(output_data_header_t));
frame_len = header.len + YIS_OUTPUT_MIN_BYTES;
if (frame_len > 255) {
break;
}
cnt -= frame_len;
pos += frame_len;
if (analysis_ok == ret) {
pthread_mutex_lock(&IMUHandler::getInstance().m_imuDataMutex);
IMUHandler::getInstance().m_imuState.quaternion()[0] =
outputInfo.attitude.quaternion_data0;
IMUHandler::getInstance().m_imuState.quaternion()[1] =
outputInfo.attitude.quaternion_data1;
IMUHandler::getInstance().m_imuState.quaternion()[2] =
outputInfo.attitude.quaternion_data2;
IMUHandler::getInstance().m_imuState.quaternion()[3] =
outputInfo.attitude.quaternion_data3;
IMUHandler::getInstance().m_imuState.gyroscope()[0] =
outputInfo.angle_rate.x * PI / 180;
IMUHandler::getInstance().m_imuState.gyroscope()[1] =
outputInfo.angle_rate.y * PI / 180;
IMUHandler::getInstance().m_imuState.gyroscope()[2] =
outputInfo.angle_rate.z * PI / 180;
IMUHandler::getInstance().m_imuState.accelerometer()[0] =
outputInfo.accel.x;
IMUHandler::getInstance().m_imuState.accelerometer()[1] =
outputInfo.accel.y;
IMUHandler::getInstance().m_imuState.accelerometer()[2] =
outputInfo.accel.z;
IMUHandler::getInstance().m_imuState.rpy()[0] =
outputInfo.attitude.roll * PI / 180;
IMUHandler::getInstance().m_imuState.rpy()[1] =
outputInfo.attitude.pitch * PI / 180;
IMUHandler::getInstance().m_imuState.rpy()[2] =
outputInfo.attitude.yaw * PI / 180;
IMUHandler::getInstance().m_imuState.temperature() =
static_cast(outputInfo.sensor_temp);
pthread_mutex_unlock(&IMUHandler::getInstance().m_imuDataMutex);
}
}
}
memmove(recvBuf, recvBuf + pos, cnt);
recvBufIdx = cnt;
tcflush(IMUHandler::getInstance().m_serialFd, TCIFLUSH);
std::this_thread::sleep_for(2ms); // line 208
if (IMUHandler::getInstance().m_isIumNeedCalibrate) {
// 发送校准数据
unsigned char outCalcmd[] = {0x59, 0x53, 0x4d, 0x12, 0x00,
0x50, 0x01, 0xb0, 0x6a}; // 校准指令
int bytesSend = 0;
bytesSend = write(IMUHandler::getInstance().m_serialFd, outCalcmd,
sizeof(outCalcmd));
if (bytesSend == sizeof(outCalcmd)) {
LOG("imu calibrate successfully!\n");
} else {
ERR("failed to calibrate imu, bytesSend={}\n", bytesSend);
}
IMUHandler::getInstance().SetCalibrateFlag(false);
}
} catch (const std::exception& e) {
ERR("Exception in ReadIMU loop: {}.", e.what());
}
}
IMUHandler::getInstance().CloseIMU();
pthread_exit(NULL);
}
Подробнее здесь: https://stackoverflow.com/questions/797 ... as-a-crash