diff --git a/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h b/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h index e481607b0c1..d3c7efa2e27 100755 --- a/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h +++ b/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h @@ -69,6 +69,8 @@ enum EPOLLDATATYPE { E_POLL_GNSS_STATE = 20, E_POLL_IMU_STATE = 40, E_POLL_CAN_STATE = 60, + E_POLL_INS_STANDARD_GNSS2 = 90, + E_POLL_INS_STANDARD_ODS2 = 95, E_POLL_INS_STATE = 80, E_POLL_GNSS2_STATE = 100, E_POLL_DATA_TOTAL = 120, @@ -79,6 +81,7 @@ enum MessageId : uint16_t { BIN_SINS_DATA = 0x02AA, BIN_IMU_DATA = 0x03AA, BIN_GNSS_DATA = 0x04AA, + BIN_Extend_DATA = 0x05AA, }; // Every binary message has 32-bit CRC performed on all data including the @@ -140,7 +143,7 @@ struct NAV_DATA_TypeDef { int16_t poll_frame2; int16_t poll_frame3; }; -static_assert(sizeof(NAV_DATA_TypeDef) == 65, "Incorrect NAV_DATA_TypeDef"); +// static_assert(sizeof(NAV_DATA_TypeDef) == 65, "Incorrect NAV_DATA_TypeDef"); struct NAV_SINS_TypeDef { uint32_t gps_week; // GPS Week number. @@ -166,7 +169,7 @@ struct NAV_SINS_TypeDef { float xigema_roll; // roll std unit degree float xigema_head; // heading std unit degree }; -static_assert(sizeof(NAV_SINS_TypeDef) == 90, "Incorrect NAV_SINS_TypeDef"); +// static_assert(sizeof(NAV_SINS_TypeDef) == 90, "Incorrect NAV_SINS_TypeDef"); struct NAV_IMU_TypeDef { uint32_t gps_week; // GPS Week number. @@ -185,7 +188,7 @@ struct NAV_IMU_TypeDef { double magnetY; // unit mGauss double magnetZ; // unit mGauss }; -static_assert(sizeof(NAV_IMU_TypeDef) == 87, "Incorrect NAV_IMU_TypeDef"); +// static_assert(sizeof(NAV_IMU_TypeDef) == 87, "Incorrect NAV_IMU_TypeDef"); struct NAV_GNSS_TypeDef { uint32_t gps_week; // GPS Week number. @@ -209,8 +212,25 @@ struct NAV_GNSS_TypeDef { float xigema_lat; // North pos std unit m float xigema_lon; // East pos std unit m float xigema_alt; // up pos std unit m + float xigema_heading; // heading std unit degree }; -static_assert(sizeof(NAV_GNSS_TypeDef) == 80, "Incorrect NAV_GNSS_TypeDef"); +// static_assert(sizeof(NAV_GNSS_TypeDef) == 84, "Incorrect NAV_GNSS_TypeDef"); + +struct NAV_Extend_TypeDef { + uint32_t gps_week; // GPS Week number. + uint32_t gpssecond; // Milliseconds of week. + float corrGyroX; + float corrGyroY; + float corrGyroZ; + float corrAccX; + float corrAccY; + float corrAccZ; + float gnssAttFromIMU; // GNSS angle precision compensation from IMU + float odsAttFromIMU; // Carrier angle compensation from IMU + float Undulation; // Geoidal separation +}; +// static_assert(sizeof(NAV_Extend_TypeDef) == 44, "Incorrect +// NAV_Extend_TypeDef"); #pragma pack(pop) diff --git a/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc b/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc index 80d9e2f4aa9..2d287414e1e 100755 --- a/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc +++ b/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc @@ -40,7 +40,7 @@ namespace drivers { namespace gnss { // Encoding and decoding protocol scaling factor setting -#define Coder_Accel_Scale 12.0 +#define Coder_Accel_Scale 120.0 #define Coder_Rate_Scale 300.0 #define Coder_MAG_Scale 1.0 #define Coder_IFof_Rate_Scale 600.0 @@ -53,6 +53,25 @@ namespace gnss { #define Coder_Pos_Scale 10000000000.0 constexpr size_t BUFFER_SIZE = 256; +uint16_t g_ENS_MSG_ID = 0; + +union { + char bd[8]; + unsigned short iv; + short sv; + int lv; + unsigned int uv; + float fv; + double dv; +} m_uMemory; + +float get_F32(char* msgbuff, int i) { + m_uMemory.bd[0] = msgbuff[i]; + m_uMemory.bd[1] = msgbuff[i + 1]; + m_uMemory.bd[2] = msgbuff[i + 2]; + m_uMemory.bd[3] = msgbuff[i + 3]; + return m_uMemory.fv; +} class EnbroadParse : public Parser { public: @@ -66,6 +85,7 @@ class EnbroadParse : public Parser { bool check_sum(); bool HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData); bool HandleSINSData(const enbroad::NAV_SINS_TypeDef* pSinsData); + bool HandleExtendData(const enbroad::NAV_Extend_TypeDef* pExtendData); bool HandleIMUData(const enbroad::NAV_IMU_TypeDef* pImuData); bool HandleGNSSData(const enbroad::NAV_GNSS_TypeDef* pGnssData); @@ -134,17 +154,34 @@ void EnbroadParse::GetMessages(MessageInfoVec* messages) { } else { buffer_.clear(); total_length_ = 0; - messages->push_back( - MessageInfo{MessageType::BEST_GNSS_POS, - reinterpret_cast(&bestpos_)}); - messages->push_back( - MessageInfo{MessageType::IMU, reinterpret_cast(&imu_)}); - messages->push_back(MessageInfo{ - MessageType::HEADING, reinterpret_cast(&heading_)}); - messages->push_back( - MessageInfo{MessageType::INS, reinterpret_cast(&ins_)}); - messages->push_back(MessageInfo{ - MessageType::INS_STAT, reinterpret_cast(&ins_stat_)}); + if (enbroad::BIN_NAV_DATA == g_ENS_MSG_ID) { + messages->push_back( + MessageInfo{MessageType::BEST_GNSS_POS, + reinterpret_cast(&bestpos_)}); + messages->push_back(MessageInfo{MessageType::IMU, + reinterpret_cast(&imu_)}); + messages->push_back(MessageInfo{ + MessageType::HEADING, reinterpret_cast(&heading_)}); + messages->push_back(MessageInfo{MessageType::INS, + reinterpret_cast(&ins_)}); + messages->push_back(MessageInfo{ + MessageType::INS_STAT, reinterpret_cast(&ins_stat_)}); + } else if (enbroad::BIN_Extend_DATA == + g_ENS_MSG_ID) { // ins=sins+extend + messages->push_back(MessageInfo{MessageType::INS, + reinterpret_cast(&ins_)}); + messages->push_back(MessageInfo{ + MessageType::INS_STAT, reinterpret_cast(&ins_stat_)}); + } else if (enbroad::BIN_IMU_DATA == g_ENS_MSG_ID) { + messages->push_back(MessageInfo{MessageType::IMU, + reinterpret_cast(&imu_)}); + } else if (enbroad::BIN_GNSS_DATA == g_ENS_MSG_ID) { + messages->push_back( + MessageInfo{MessageType::BEST_GNSS_POS, + reinterpret_cast(&bestpos_)}); + messages->push_back(MessageInfo{ + MessageType::HEADING, reinterpret_cast(&heading_)}); + } } } } @@ -178,23 +215,23 @@ bool EnbroadParse::check_sum() { } bool EnbroadParse::PrepareMessage() { + static uint64_t msg_sum = 0; + static uint64_t msg_bad = 0; + g_ENS_MSG_ID = 0; + msg_sum++; if (!check_sum()) { - AWARN << "check sum failed. bad frame ratio"; + msg_bad++; + AERROR << "check sum failed. bad frame ratio" + << static_cast(msg_bad / msg_sum); return false; } uint8_t* message = nullptr; enbroad::MessageId message_id; - uint16_t message_length; auto header = reinterpret_cast(buffer_.data()); message = buffer_.data() + sizeof(enbroad::FrameHeader); message_id = header->message_id; - message_length = header->message_length; switch (message_id) { case enbroad::BIN_NAV_DATA: - if (message_length != sizeof(enbroad::NAV_DATA_TypeDef)) { - AWARN << "Incorrect message_length"; - break; - } if (!HandleNavData( reinterpret_cast(message))) { AWARN << "HandleNavData fail"; @@ -202,10 +239,6 @@ bool EnbroadParse::PrepareMessage() { } break; case enbroad::BIN_SINS_DATA: - if (message_length != sizeof(enbroad::NAV_SINS_TypeDef)) { - AWARN << "Incorrect message_length"; - break; - } if (!HandleSINSData( reinterpret_cast(message))) { AWARN << "HandleSINSData fail"; @@ -213,10 +246,6 @@ bool EnbroadParse::PrepareMessage() { } break; case enbroad::BIN_IMU_DATA: - if (message_length != sizeof(enbroad::NAV_IMU_TypeDef)) { - AWARN << "Incorrect message_length"; - break; - } if (!HandleIMUData( reinterpret_cast(message))) { AWARN << "HandleIMUData fail"; @@ -224,18 +253,22 @@ bool EnbroadParse::PrepareMessage() { } break; case enbroad::BIN_GNSS_DATA: - if (message_length != sizeof(enbroad::NAV_GNSS_TypeDef)) { - AWARN << "Incorrect message_length"; - break; - } if (!HandleGNSSData( reinterpret_cast(message))) { return false; } break; + case enbroad::BIN_Extend_DATA: + if (!HandleExtendData( + reinterpret_cast(message))) { + return false; + } + break; default: return false; } + + g_ENS_MSG_ID = message_id; return true; } @@ -255,49 +288,32 @@ bool EnbroadParse::HandleSINSData(const enbroad::NAV_SINS_TypeDef* pSinsData) { ins_.mutable_linear_velocity()->set_x(pSinsData->ve); ins_.mutable_linear_velocity()->set_y(pSinsData->vn); ins_.mutable_linear_velocity()->set_z(pSinsData->vu); - if (enbroad::E_NAV_STATUS_IN_NAV == pSinsData->navStatus) { + if (2 == static_cast(pSinsData->navStatus)) { ins_.set_type(Ins::GOOD); - } else if (enbroad::E_NAV_STATUS_SYSTEM_STANDARD == pSinsData->navStatus) { + } else if (1 == static_cast(pSinsData->navStatus)) { ins_.set_type(Ins::CONVERGING); } else { ins_.set_type(Ins::INVALID); } - bestpos_.set_measurement_time(seconds); - bestpos_.set_longitude(pSinsData->longitude); - bestpos_.set_latitude(pSinsData->latitude); - bestpos_.set_height_msl(pSinsData->altitude); - // bestpos_.set_undulation(0.0);//undulation = height_wgs84 - height_msl - // datum id number.WGS84 - bestpos_.set_datum_id( - static_cast(enbroad::DatumId::WGS84)); - // sins standard deviation - bestpos_.set_latitude_std_dev(pSinsData->xigema_lat); - bestpos_.set_longitude_std_dev(pSinsData->xigema_lon); - bestpos_.set_height_std_dev(pSinsData->xigema_alt); - ins_stat_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); // ins pos type define as follows // fusion GPS as INS_RTKFIXED, // fusion wheel as INS_RTKFLOAT,fusion motion as SINGLE,others as NONE - if (enbroad::E_FUNSION_GPS == pSinsData->fusion) { + if (enbroad::E_FUNSION_GPS == static_cast(pSinsData->fusion)) { ins_stat_.set_pos_type(SolutionType::INS_RTKFIXED); - bestpos_.set_sol_type(SolutionType::INS_RTKFIXED); } else { ins_stat_.set_pos_type(SolutionType::NONE); - bestpos_.set_sol_type(SolutionType::NONE); } - if (enbroad::E_NAV_STATUS_IN_NAV == pSinsData->navStatus) { + if (2 == static_cast(pSinsData->navStatus)) { ins_stat_.set_ins_status(SolutionStatus::SOL_COMPUTED); - bestpos_.set_sol_status(SolutionStatus::SOL_COMPUTED); - } else if (enbroad::E_NAV_STATUS_SYSTEM_STANDARD == pSinsData->navStatus) { + } else if (1 == static_cast(pSinsData->navStatus)) { ins_stat_.set_ins_status(SolutionStatus::COLD_START); - bestpos_.set_sol_status(SolutionStatus::COLD_START); } else { ins_stat_.set_ins_status(SolutionStatus::INSUFFICIENT_OBS); - bestpos_.set_sol_status(SolutionStatus::INSUFFICIENT_OBS); } + return true; } bool EnbroadParse::HandleIMUData(const enbroad::NAV_IMU_TypeDef* pImuData) { @@ -318,47 +334,112 @@ bool EnbroadParse::HandleIMUData(const enbroad::NAV_IMU_TypeDef* pImuData) { bool EnbroadParse::HandleGNSSData(const enbroad::NAV_GNSS_TypeDef* pGnssData) { double seconds = pGnssData->gps_week * SECONDS_PER_WEEK + pGnssData->gpssecond * 1e-3; + + bestpos_.set_measurement_time(seconds); + heading_.set_measurement_time(seconds); // bestpos_.set_base_station_id("0"); // base station id - bestpos_.set_solution_age(pGnssData->age); // solution age (sec) bestpos_.set_num_sats_tracked(pGnssData->satsNum); bestpos_.set_num_sats_in_solution(pGnssData->satsNum); bestpos_.set_num_sats_in_solution(pGnssData->satsNum); bestpos_.set_num_sats_multi(pGnssData->satsNum); + heading_.set_satellite_tracked_number(pGnssData->satsNum); + heading_.set_satellite_soulution_number(pGnssData->satsNum); + heading_.set_satellite_number_obs(pGnssData->satsNum); + heading_.set_satellite_number_multi(pGnssData->satsNum); // bestpos_.set_galileo_beidou_used_mask(0); // bestpos_.set_gps_glonass_used_mask(0); + bestpos_.set_solution_age(pGnssData->age); // solution age (sec) + if (enbroad::E_GPS_RTK_FIXED == static_cast(pGnssData->rtkStatus)) { + bestpos_.set_sol_type(SolutionType::INS_RTKFIXED); + } else if (enbroad::E_GPS_RTK_FLOAT == + static_cast(pGnssData->rtkStatus)) { + bestpos_.set_sol_type(SolutionType::INS_RTKFLOAT); + } else if (enbroad::E_GPS_RTK_SPP == static_cast(pGnssData->rtkStatus) || + enbroad::E_GPS_RTK_DGPS == + static_cast(pGnssData->rtkStatus)) { + bestpos_.set_sol_type(SolutionType::SINGLE); + } else { + bestpos_.set_sol_type(SolutionType::NONE); + } - heading_.set_measurement_time(seconds); - heading_.set_heading(pGnssData->heading); + bestpos_.set_latitude(pGnssData->latitude); + bestpos_.set_longitude(pGnssData->longitude); + bestpos_.set_height_msl(pGnssData->altitude); + + if (enbroad::E_GPS_RTK_FIXED == static_cast(pGnssData->headingStatus)) { + heading_.set_position_type(SolutionType::INS_RTKFIXED); + } else if (enbroad::E_GPS_RTK_FLOAT == + static_cast(pGnssData->headingStatus)) { + heading_.set_position_type(SolutionType::INS_RTKFLOAT); + } else if (enbroad::E_GPS_RTK_SPP == + static_cast(pGnssData->headingStatus) || + enbroad::E_GPS_RTK_DGPS == + static_cast(pGnssData->headingStatus)) { + heading_.set_position_type(SolutionType::SINGLE); + } else { + heading_.set_position_type(SolutionType::NONE); + } heading_.set_baseline_length(pGnssData->baseline); + heading_.set_heading(pGnssData->heading); heading_.set_reserved(0); + heading_.set_heading_std_dev(pGnssData->xigema_heading); // heading_.set_heading_std_dev(0.0); // heading_.set_pitch_std_dev(0.0); // heading_.set_station_id("0"); - heading_.set_satellite_tracked_number(pGnssData->satsNum); - heading_.set_satellite_soulution_number(pGnssData->satsNum); - heading_.set_satellite_number_obs(pGnssData->satsNum); - heading_.set_satellite_number_multi(pGnssData->satsNum); + // heading_.set_solution_source(0); // heading_.set_extended_solution_status(0); // heading_.set_galileo_beidou_sig_mask(0); // heading_.set_gps_glonass_sig_mask(0); - if (enbroad::E_GPS_RTK_FIXED == pGnssData->headingStatus) { - heading_.set_position_type(SolutionType::INS_RTKFIXED); - } else if (enbroad::E_GPS_RTK_FLOAT == pGnssData->headingStatus) { - heading_.set_position_type(SolutionType::INS_RTKFLOAT); - } else if (enbroad::E_GPS_RTK_SPP == pGnssData->headingStatus || - enbroad::E_GPS_RTK_DGPS == pGnssData->headingStatus) { - heading_.set_position_type(SolutionType::SINGLE); - } else { - heading_.set_position_type(SolutionType::NONE); - } + + // AERROR << "pGnssData->xigema_lat=" << pGnssData->xigema_lat; + // AERROR << "pGnssData->xigema_lon=" << pGnssData->xigema_lon; + // AERROR << "ppGnssData->xigema_alt=" << pGnssData->xigema_alt; + // AERROR << "ppGnssData->xigema_heading=" << pGnssData->xigema_heading; + return true; } + +// *Strongly recommend: +// Do not place NAV_Extend_TypeDef in imu_, heading_, bestpos_ +// to avoid data anomalies +bool EnbroadParse::HandleExtendData( + const enbroad::NAV_Extend_TypeDef* pExtendData) { + ins_.mutable_linear_acceleration()->set_x( + static_cast(pExtendData->corrAccX)); + ins_.mutable_linear_acceleration()->set_y( + static_cast(pExtendData->corrAccY)); + ins_.mutable_linear_acceleration()->set_z( + static_cast(pExtendData->corrAccZ)); + ins_.mutable_angular_velocity()->set_x( + static_cast(pExtendData->corrGyroX * DEG_TO_RAD)); + ins_.mutable_angular_velocity()->set_y( + static_cast(pExtendData->corrGyroY * DEG_TO_RAD)); + ins_.mutable_angular_velocity()->set_z( + static_cast(pExtendData->corrGyroZ * DEG_TO_RAD)); +#if 0 + AERROR << "pExtendData->corrAccX=" << pExtendData->corrAccX; + AERROR << "pExtendData->corrAccY=" << pExtendData->corrAccY; + AERROR << "pExtendData->corrAccZ=" << pExtendData->corrAccZ; + + AERROR << "pExtendData->corrGyroX=" << pExtendData->corrGyroX; + AERROR << "pExtendData->corrGyroY=" << pExtendData->corrGyroY; + AERROR << "pExtendData->corrGyroZ=" << pExtendData->corrGyroZ; + + AERROR << "pExtendData->gnssAttFromIMU=" << pExtendData->gnssAttFromIMU; + AERROR << "pExtendData->odsAttFromIMU=" << pExtendData->odsAttFromIMU; + + AERROR << "pExtendData->Undulation=" << pExtendData->Undulation; +#endif + return true; +} + bool EnbroadParse::HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData) { static uint16_t rtkStatus; static uint16_t Nav_Standard_flag; static uint16_t Sate_Num; static float baseline; + // char buff[8]={0}; float imu_measurement_span = 1.0f / 100.0f; double seconds = pNavData->gps_week * SECONDS_PER_WEEK + pNavData->gps_millisecs * 1e-3; @@ -412,6 +493,14 @@ bool EnbroadParse::HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData) { break; case enbroad::E_POLL_INS_STATE: Nav_Standard_flag = pNavData->poll_frame1; + case enbroad::E_POLL_INS_STANDARD_GNSS2: + // poll_frame1+poll_frame2×éfloat + // char *pAddr= static_cast(&pNavData->poll_frame1); + // buff[0]=pNavData->poll_frame1&0XFF00; + // buff[1]=pNavData->poll_frame1&0X00FF; + // buff[2]=pNavData->poll_frame2&0XFF00; + // buff[3]=pNavData->poll_frame2&0X00FF; + // AERROR << "E_POLL_INS_STANDARD_GNSS2=" << get_F32(pAddr,0); break; case enbroad::E_POLL_GNSS2_STATE: Sate_Num = pNavData->poll_frame1;