#include #include #include #include #include static double cast_8_byte_to_double(const uint8_t *b); static float cast_4_byte_to_float(const uint8_t *b); static int32_t cast_3_byte_to_int32(const uint8_t *b); void iv::perception::GPSSensor::processSensor() { return; //todo GPS/惯导 设备接口 对接 /*Initialize udp server, listen on port 3000.*/ /*int sockfd; struct sockaddr_in addr; socklen_t addrlen; sockfd = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd < 0) { printf("socket failed\n"); exit(EXIT_FAILURE); } addrlen = sizeof(struct sockaddr_in); bzero(&addr, addrlen); addr.sin_family = AF_INET; addr.sin_addr.s_addr = htonl(INADDR_ANY); addr.sin_port = htons(3000); if (bind(sockfd, (struct sockaddr*)(&addr), addrlen) < 0) { printf("bind fail\n"); exit(EXIT_FAILURE); }*/ QUdpSocket *udpsocket;//summer udpsocket = new QUdpSocket(); udpsocket->bind(QHostAddress::Any, 3000); // udpsocket->bind(3000); /* if(!udpsocket->waitForConnected()) { printf("bind fail\n"); exit(EXIT_FAILURE); }*/ char *Buf = new char[100] ;//summer unsigned char - char memset(Buf,0,100); unsigned char recvBuf[100] = {0}; /*UDP initialization done. Next prepare for receiving data.*/ iv::GPSData data(new iv::GPS_INS); int x, y, z; //iv::CarStatus currentCarStatus; //ODS("\n\n\n------>thread gps: %X\n\n\n\n", GetCurrentThreadId()); while (!boost::this_thread::interruption_requested()) { int rec = 0; if(udpsocket->hasPendingDatagrams()) { // std::cout<<"have data."<receiveDatagram(); QByteArray ba = datagram.data(); if(ba.size() != NOUTPUT_PACKET_LENGTH) { continue; } else { rec = NOUTPUT_PACKET_LENGTH; memcpy(recvBuf,ba.data(),NOUTPUT_PACKET_LENGTH); } datagram.clear(); } else { #ifdef Q_OS_LINUX usleep(1000); #endif #ifdef WIN32 boost::this_thread::sleep(boost::posix_time::milliseconds(1)); // Sleep(5); #endif continue; // std::cout<<"running."<waitForReadyRead()) rec = udpsocket->read(Buf,100); //recvBuf = (unsigned char*)Buf; convertStrToUnChar(Buf,recvBuf);//summer */ //int rec = recvfrom(sockfd, recvBuf, 100, 0, (struct sockaddr *)(&addr), &addrlen); /*Check the receiving data*/ //There must be 72 bytes in one package. if (rec != NOUTPUT_PACKET_LENGTH) { printf("ERR: rec must be 72 bytes\n"); continue; } if (recvBuf[PI_SYNC] != NCOM_SYNC) { printf("ERR: head always be 0xE7\n"); continue; } data->ins_status = recvBuf[PI_INS_NAV_MODE]; if(data->ins_status == 0x0B)continue; //31.1150882 121.2211320 356.9 //31.1150844 121.2210996 356.9 VV7_3 /* Start decoding Details in ncomman.pdf Byte | 0 | 1-20 | 21 | 22 | 23-60 | 61 | 62 | 63-70 | 71 Description | Sync | BatchA | NaviStatus | ChkSum1 | BatchB | ChkSum2 | StatusCh | BatchS | ChkSum3 */ //start decoding -> Batch A //Time is transmitted as milliseconds into the current GPS minute. The range is 0–59, 999 ms. /* Acceleration x is the host object’s acceleration in the x-direction (i.e.after the IMU to host attitude matrix has been applied). It is a signed word in units of 1 × 10^(−4) m / (s^2) */ x = cast_3_byte_to_int32(recvBuf + PI_ACCEL_X); y = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Y); z = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Z); if (x != INV_INT_24) data->accel_x = x * ACC2MPS2; if (y != INV_INT_24) data->accel_y = y * ACC2MPS2; if (z != INV_INT_24) data->accel_z = z * ACC2MPS2; /* Angular rate x is the host object’s angular rate about its x-axis (i.e. after the IMU to host attitude matrix has been applied). It is a signed word in units of 1 × 10−5 radians/s */ x = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_X); y = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Y); z = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Z); if (x != INV_INT_24) data->ang_rate_x = x * (RATE2RPS * RAD2DEG); if (y != INV_INT_24) data->ang_rate_y = y * (RATE2RPS * RAD2DEG); if (z != INV_INT_24) data->ang_rate_z = z * (RATE2RPS * RAD2DEG); /* The navigation status byte value should be 0–7, 10 or 20–22 to be valid for customer use. See page 10. A value of 11 indicates the packet follows NCOM structure-B and should be ignored. */ data->ins_status = recvBuf[PI_INS_NAV_MODE]; //navi_status refer to imu status 2 3 4. //check this out in page 11 table 5. /* Checksum 1 allows the software to verify the integrity of bytes 1–21. The sync byte is ignored. In low-latency applications the inertial measurements in Batch A can be used to update a previous solution without waiting for the rest of the packet to be received. Contact Oxford Technical Solutions for source code to perform this function. */ //31.1150796 121.2211047 355.8 //start decoding Batch B /* Position, orientation and velocity output. See Table 6, for a detailed description. */ /*Byte23-30. The Latitude of the INS. It is a double in units of radians.*/ /*Byte31-38. Longitude of the INS. It is a double in units of radians.*/ /*Byte39-42. Altitude of the INS. It is a float in units of metres.*/ /*double latitude; double longitude; memcpy(&latitude, recvBuf + 23, sizeof(double)); memcpy(&longitude, recvBuf + 31, sizeof(double)); latitude = latitude * 180.0 / M_PI; longitude = longitude * 180.0 / M_PI; printf("lat= %.20lf lon= %.20lf | sizeof(float)=%d\n", latitude, longitude, sizeof(float)); */ data->gps_lat = cast_8_byte_to_double(recvBuf + PI_POS_LAT) * RAD2DEG; data->gps_lng = cast_8_byte_to_double(recvBuf + PI_POS_LON) * RAD2DEG; data->gps_z = cast_4_byte_to_float(recvBuf + PI_POS_ALT); /*Byte43-45. Byte46-48. Byte49-51. Velocity in units of 1 × 10^−4 m/s.*/ x = cast_3_byte_to_int32(recvBuf + PI_VEL_N); y = cast_3_byte_to_int32(recvBuf + PI_VEL_E); z = cast_3_byte_to_int32(recvBuf + PI_VEL_D); if (x != INV_INT_24) data->vel_N = x * VEL2MPS; if (y != INV_INT_24) data->vel_E = y * VEL2MPS; if (z != INV_INT_24) data->vel_D = z * VEL2MPS; /*Byte52-54. Heading in units of 1 × 10−6 radians. Range ±pi.*/ /*Byte55-57. Pitch in units of 1 × 10−6 radians. Range ±pi/2.*/ /*Byte58-60. Roll in units of 1 × 10−6 radians. Range ±pi.*/ x = cast_3_byte_to_int32(recvBuf + PI_ORIEN_H); y = cast_3_byte_to_int32(recvBuf + PI_ORIEN_P); z = cast_3_byte_to_int32(recvBuf + PI_ORIEN_R); if (x != INV_INT_24) data->ins_heading_angle = x * (ANG2RAD * RAD2DEG); if (y != INV_INT_24) data->ins_pitch_angle = y * (ANG2RAD * RAD2DEG); if (z != INV_INT_24) data->ins_roll_angle = z * (ANG2RAD * RAD2DEG); #ifdef VV7_2 data->ins_heading_angle = data->ins_heading_angle+1.9; #endif #ifdef VV7_3 data->ins_heading_angle = data->ins_heading_angle-1.0; #endif if(data->ins_heading_angle>360)data->ins_heading_angle = data->ins_heading_angle-360; if (data->ins_heading_angle < 0.0) data->ins_heading_angle += 360.0; if (recvBuf[PI_CHANNEL_INDEX] == PI_CHANNEL_INDEX) { data->rtk_status = recvBuf[PI_RTK_STATUS]; data->gps_satelites_num = recvBuf[PI_SAT_NUM]; } ServiceCarStatus.mRTKStatus = data->rtk_status; if (data->ins_status != 4) { data->valid = IMU_STATUS_ERR; } else if (data->rtk_status != 3 && data->rtk_status != 5 && data->rtk_status != 6) { data->valid = RTK_STATUS_ERR; } else { data->valid = RTK_IMU_OK; } GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y); ////从can中读取当前车速 //data->speed = ServiceCarStatus.speed; if(data->ins_status != 0x0B)data->speed = sqrt(data->vel_N*data->vel_N + data->vel_E * data->vel_E)*3.6; else continue; if(data->gps_lat<0||data->gps_lat>90){ continue; } ServiceCarStatus.speed = data->speed; // qDebug("speed x is %g",ServiceCarStatus.speed); if(data->ins_status == 4) signal_gps_data->operator()(data); //传输数据 ServiceCarStatus.location->gps_lat = data->gps_lat; ServiceCarStatus.location->gps_lng = data->gps_lng; ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle; ServiceCarStatus.location->gps_x = data->gps_x; ServiceCarStatus.location->gps_y = data->gps_y; ServiceCarStatus.location->rtk_status = data->rtk_status; ServiceCarStatus.location->ins_status = data->ins_status; // std::cout<<"lat = "<gps_lat<close(); } static double cast_8_byte_to_double(const uint8_t *b) { union { double x; uint8_t c[8]; } u; u.c[0] = b[0]; u.c[1] = b[1]; u.c[2] = b[2]; u.c[3] = b[3]; u.c[4] = b[4]; u.c[5] = b[5]; u.c[6] = b[6]; u.c[7] = b[7]; return u.x; } static float cast_4_byte_to_float(const uint8_t *b) { union { float x; uint8_t c[4]; } u; u.c[0] = b[0]; u.c[1] = b[1]; u.c[2] = b[2]; u.c[3] = b[3]; return u.x; } static int32_t cast_3_byte_to_int32(const uint8_t *b) { union { int32_t x; uint8_t c[4]; } u; u.c[1] = b[0]; u.c[2] = b[1]; u.c[3] = b[2]; return u.x >> 8; } void iv::perception::GPSSensor::convertStrToUnChar(char* str, unsigned char* UnChar) { int i = strlen(str), j = 0, counter = 0; char c[2]; unsigned int bytes[2]; for (j = 0; j < i; j += 2) { if(0 == j % 2) { c[0] = str[j]; c[1] = str[j + 1]; sscanf(c, "%02x" , &bytes[0]); UnChar[counter] = bytes[0]; counter++; } } } void iv::perception::GPSSensor::UpdateGPSIMU(iv::gps::gpsimu xgpsimu) { iv::GPSData data(new iv::GPS_INS); data->ins_status = xgpsimu.ins_state(); data->rtk_status = xgpsimu.rtk_state(); data->accel_x = xgpsimu.acce_x(); data->accel_y = xgpsimu.acce_y(); data->accel_z = xgpsimu.acce_z(); data->ang_rate_x = xgpsimu.gyro_x(); data->ang_rate_y = xgpsimu.gyro_y(); data->ang_rate_z = xgpsimu.gyro_z(); data->gps_lat = xgpsimu.lat(); data->gps_lng = xgpsimu.lon(); data->gps_z = xgpsimu.height(); data->ins_heading_angle = xgpsimu.heading(); data->ins_pitch_angle = xgpsimu.pitch(); data->ins_roll_angle = xgpsimu.roll(); data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6; GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y); ServiceCarStatus.mRTKStatus = data->rtk_status; ServiceCarStatus.mfGPSAcc = xgpsimu.acc_calc(); ServiceCarStatus.speed = data->speed; // qDebug("speed x is %g",ServiceCarStatus.speed); if(data->ins_status == 4) { signal_gps_data->operator()(data); //传输数据 } ServiceCarStatus.location->gps_lat = data->gps_lat; ServiceCarStatus.location->gps_lng = data->gps_lng; ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle; ServiceCarStatus.location->gps_x = data->gps_x; ServiceCarStatus.location->gps_y = data->gps_y; ServiceCarStatus.location->rtk_status = data->rtk_status; ServiceCarStatus.location->ins_status = data->ins_status; }