From 11227051506933af23c3733e0be51a7a09f86721 Mon Sep 17 00:00:00 2001 From: dengwei19840721 Date: Tue, 23 Apr 2024 13:37:38 +0800 Subject: [PATCH 1/5] feat:add driver_gnss_Enbroad to driver_gnss --- modules/drivers/gnss/conf/gnss_conf.pb.txt | 20 +- .../parser/enbroad_parser/enbroad_messages.h | 218 +++++++++ .../parser/enbroad_parser/enbroad_parser.cc | 426 ++++++++++++++++++ modules/drivers/gnss/parser/parser.h | 3 + modules/drivers/gnss/proto/config.proto | 4 + 5 files changed, 661 insertions(+), 10 deletions(-) create mode 100755 modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h create mode 100755 modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc diff --git a/modules/drivers/gnss/conf/gnss_conf.pb.txt b/modules/drivers/gnss/conf/gnss_conf.pb.txt index b1e712af8b0..949d7e6626e 100644 --- a/modules/drivers/gnss/conf/gnss_conf.pb.txt +++ b/modules/drivers/gnss/conf/gnss_conf.pb.txt @@ -1,11 +1,11 @@ data { - # format optional: NOVATEL_BINARY, HUACE_TEXT, ASENSING_BINARY, BROADGNSS_TEXT - format: NOVATEL_BINARY + # format optional: NOVATEL_BINARY, HUACE_TEXT, ASENSING_BINARY, BROADGNSS_TEXT,ENBROAD_BINARY + format: ENBROAD_BINARY # protocol optional: serial, tcp, udp, ntrip, can_card_parameter serial { - device: "/dev/ttyACM0" - baud_rate: 115200 + device: "/dev/ttyUSB0" + baud_rate: 460800 } # tcp, udp: e.g. @@ -37,18 +37,18 @@ data { #} rtk_to { - format: NOVATEL_BINARY + format: ENBROAD_BINARY serial { - device: "/dev/ttyACM1" - baud_rate: 115200 + device: "/dev/ttyUSB0" + baud_rate: 460800 } } command { - format: NOVATEL_BINARY + format: ENBROAD_BINARY serial { - device: "/dev/ttyACM2" - baud_rate: 115200 + device: "/dev/ttyUSB0" + baud_rate: 460800 } } diff --git a/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h b/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h new file mode 100755 index 00000000000..56c3897d5f3 --- /dev/null +++ b/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h @@ -0,0 +1,218 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +// This defines enums and structures for parsing ENS binary messages. Please +// refer to ENS's +// documents for details about these messages. + + +#pragma once + +#include +#include + +#include "modules/drivers/gnss/proto/config.pb.h" + +namespace apollo { +namespace drivers { +namespace gnss { +namespace enbroad { + +enum ENAVSTANDARDFLAGSTATUS +{ + E_NAV_STANDARD_NO_PROCCSS=0, //0:未标定完成 + E_NAV_STANDARD_PROCCSSING=1, //1：标定中 + E_NAV_STANDARD_PROCCSSED=2, //2：标定完成 + E_NAV_STANDARD_TOTAL=3, //总共状态数目 +}; + +enum ENAVSTATUS +{ + E_NAV_STATUS_START =0, //开始导航加载导航参数 + E_NAV_STATUS_ROUTH_ALIGN=1, //SINS初对准 + E_NAV_STATUS_SINS_KF_INITIAL=2, //SINS、KF初始化 + E_NAV_STATUS_SYSTEM_STANDARD=3, //系统标定 + E_NAV_STATUS_IN_NAV=4, //正常导航 + E_NAV_STATUS_STOP=5, //停止导航 + E_NAV_STATUS_TOTAL=6, //总共状态数目 +}; + +//rtk状态 +enum EGPSRTKSTATUS +{ + E_GPS_RTK_INVALID=0, //0: 无定位 + E_GPS_RTK_SPP=1, //1：单点定位 + E_GPS_RTK_DGPS=2, //2：差分解 + E_GPS_RTK_FIXED=4, //4：RTK固定解 + E_GPS_RTK_FLOAT=5, //5：RTK浮动解 + E_GPS_RTK_TOTAL=6, + +}; + +enum EPOLLDATATYPE +{ + E_POLL_DEV_TEMP =0, + E_POLL_GNSS_STATE=20, + E_POLL_IMU_STATE=40, + E_POLL_CAN_STATE=60, + E_POLL_INS_STATE=80, + E_POLL_GNSS2_STATE=100, + E_POLL_DATA_TOTAL=120, +}; + + +enum MessageId : uint16_t { + BIN_NAV_DATA = 0x01AA, + BIN_SINS_DATA = 0x02AA, + BIN_IMU_DATA = 0x03AA, + BIN_GNSS_DATA = 0x04AA, +}; + +// Every binary message has 32-bit CRC performed on all data including the +// header. +//constexpr uint16_t CRC_LENGTH = 4; + +#pragma pack(push, 1) // Turn off struct padding. + +enum class DatumId : uint32_t { + // We only use WGS-84. + WGS84 = 61, +}; + + +//帧头 +enum SyncHeadByte : uint8_t { + SYNC_HEAD_0 = 0xA5, + SYNC_HEAD_1 = 0x5A, + SYNC_HEAD_2 = 0x5A, +}; +//帧尾 +enum SyncTailByte : uint8_t { + SYNC_TAIL_0 = 0xA5, + SYNC_Tail_1 = 0x5A, + SYNC_Tail_2 = 0x5A, +}; + +//Frame Head +struct FrameHeader{ + SyncHeadByte sync_head[3];//sync Head + uint8_t src_id;//src id + uint8_t dst_id;//dst id + MessageId message_id;//message id + uint8_t rsp; //RSP + uint16_t SN; //session NO. + uint16_t reserved;//RESERVED + uint16_t message_length; // Message length +}; + +static_assert(sizeof(FrameHeader) == 14, "Incorrect size of FrameHeader"); +struct NAV_DATA_TypeDef{ + + uint32_t gps_week; // GPS Week number. + uint32_t gps_millisecs; // Milliseconds of week. + uint16_t pitch; + uint16_t roll; + uint16_t head; + uint16_t gyroX; + uint16_t gyroY; + uint16_t gyroZ; + uint16_t accX; + uint16_t accY; + uint16_t accZ; + uint16_t magnetX; + uint16_t magnetY; + uint16_t magentZ; + long long lat; + long long lon; + uint32_t alt; + uint16_t ve; + uint16_t vn; + uint16_t vu; + uint8_t poll_type; + uint16_t poll_frame1; + uint16_t poll_frame2; + uint16_t poll_frame3; +}; +static_assert(sizeof(NAV_DATA_TypeDef) == 65, "Incorrect size of NAV_DATA_TypeDef"); + +struct NAV_SINS_TypeDef{ + + uint32_t gps_week; // GPS Week number. + uint32_t gps_millisecs; // Milliseconds of week. + + double pitch; + double roll; + double heading; + double ve; //m/s + double vn; + double vu; + double latitude; //degree + double longitude; + double altitude; + +}; +static_assert(sizeof(NAV_SINS_TypeDef) == 80, "Incorrect size of NAV_SINS_TypeDef"); + +struct NAV_IMU_TypeDef{ + + uint32_t gps_week; // GPS Week number. + uint32_t gps_millisecs; // Milliseconds of week. + + uint8_t gyroFlag; //角速度计状态0：异常，1：正常 + double gyroX; //degree/s + double gyroY; + double gyroZ; + + uint8_t accFlag; //0：异常，1：正常 + double accX; //m/s2 + double accY; + double accZ; + + uint8_t magnetFlag; //0：异常，1：正常 + double magnetX; //mGauss + double magnetY; + double magnetZ; +}; +static_assert(sizeof(NAV_IMU_TypeDef) == 83, "Incorrect size of NAV_IMU_TypeDef"); + +struct NAV_GNSS_TypeDef{ + + uint32_t gps_week; // GPS Week number. + uint32_t gps_millisecs; // Milliseconds of week. + + uint8_t rtkStatus; // + double latitude; //degree + double longitude; + double altitude; + + uint8_t headingStatus; + double heading; + + uint8_t velStatus; + double ve; //m/s + double vn; + double vu; +}; +static_assert(sizeof(NAV_GNSS_TypeDef) == 67, "Incorrect size of NAV_GNSS_TypeDef"); + + +#pragma pack(pop) + + +} // namespace enbroad +} // namespace gnss +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc b/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc new file mode 100755 index 00000000000..0fecdac1858 --- /dev/null +++ b/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc @@ -0,0 +1,426 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +// An parser for decoding binary messages from a ENS receiver. The following +// messages must be +// logged in order for this parser to work properly. + +#include +#include +#include +#include +#include + +#include "modules/common_msgs/sensor_msgs/gnss.pb.h" +#include "modules/common_msgs/sensor_msgs/gnss_best_pose.pb.h" +#include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h" +#include "modules/common_msgs/sensor_msgs/heading.pb.h" +#include "modules/common_msgs/sensor_msgs/imu.pb.h" +#include "modules/common_msgs/sensor_msgs/ins.pb.h" + +#include "cyber/cyber.h" +#include "modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h" +#include "modules/drivers/gnss/parser/parser.h" +#include "modules/drivers/gnss/parser/parser_common.h" + + +#if 1 + +namespace apollo { +namespace drivers { +namespace gnss { + + +//编解码协议比例因子设置 +#define Coder_Accel_Scale 12.0 //加计比例因子 +#define Coder_Rate_Scale 300.0 //陀螺比列因子 +#define Coder_MAG_Scale 1.0 //磁力计比列因子，待定 +#define Coder_IFof_Rate_Scale 600.0 +#define Coder_Angle_Scale 360.0 //俯仰、横滚、航向角度比列因子 +#define Coder_Temp_Scale 200.0 //温度比列因子 +#define Coder_Sensor_Scale 32768.0 //传感器比列因子 +#define Coder_IFof_Sensor_Scale 2147483648.0 +#define Coder_EXP_E 2.718282.0 +#define Coder_Vel_Scale 100.0 //速度比列因子 +#define Coder_Pos_Scale 10000000000.0 //经纬高程比例因子 + +constexpr size_t BUFFER_SIZE = 256; + +class EnbroadParse : public Parser { + public: + EnbroadParse(); + explicit EnbroadParse(const config::Config& config); + + virtual void GetMessages(MessageInfoVec *messages); + + private: + bool PrepareMessage(); + bool check_sum(); + bool HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData); + bool HandleSINSData(const enbroad::NAV_SINS_TypeDef* pSinsData); + bool HandleIMUData(const enbroad::NAV_IMU_TypeDef* pImuData); + bool HandleGNSSData(const enbroad::NAV_GNSS_TypeDef* pGnssData); + + + + size_t header_length_ = 0; + size_t total_length_ = 0; + std::vector buffer_; + + GnssBestPose bestpos_; + Imu imu_; + Heading heading_; + Ins ins_; + InsStat ins_stat_; +}; + +Parser* Parser::CreateEnbroad(const config::Config& config) { + return new EnbroadParse(config); +} + +EnbroadParse::EnbroadParse() { + buffer_.reserve(BUFFER_SIZE); + //ins_.mutable_position_covariance()->Resize(9, FLOAT_NAN); + //ins_.mutable_euler_angles_covariance()->Resize(9, FLOAT_NAN); + //ins_.mutable_linear_velocity_covariance()->Resize(9, FLOAT_NAN); + +} + +EnbroadParse::EnbroadParse(const config::Config& config) { + buffer_.reserve(BUFFER_SIZE); + //ins_.mutable_position_covariance()->Resize(9, FLOAT_NAN); + //ins_.mutable_euler_angles_covariance()->Resize(9, FLOAT_NAN); + //ins_.mutable_linear_velocity_covariance()->Resize(9, FLOAT_NAN); + + //if (config.has_imu_type()) { + // imu_type_ = config.imu_type(); + //} +} + +void EnbroadParse::GetMessages(MessageInfoVec *messages) +{ + if (data_ == nullptr) { + return; + } + while (data_ < data_end_) { + if (buffer_.empty()) { // Looking for SYNC_HEAD_0 + if (*data_ == enbroad::SYNC_HEAD_0) { + buffer_.push_back(*data_); + } + ++data_; + } else if (buffer_.size() == 1) { // Looking for SYNC_HEAD_1 + if (*data_ == enbroad::SYNC_HEAD_1) { + buffer_.push_back(*data_++); + } else { + buffer_.clear(); + } + }else if (buffer_.size() == 2) { // Looking for SYNC_HEAD_2 + if (*data_ == enbroad::SYNC_HEAD_2) { + buffer_.push_back(*data_++); + header_length_ = sizeof(enbroad::FrameHeader); + } else { + buffer_.clear(); + } + } + else if (header_length_ > 0) { // Working on header. + if (buffer_.size() < header_length_) { + buffer_.push_back(*data_++); + } else { + + //header_length + datalen + sumcheck + taillength + total_length_ = header_length_ + reinterpret_cast(buffer_.data())->message_length + 1 + 2; + header_length_ = 0; + } + } else if (total_length_ > 0) { + if (buffer_.size() < total_length_) { // Working on body. + buffer_.push_back(*data_++); + continue; + } + if(!PrepareMessage()) + { + buffer_.clear(); + total_length_ = 0; + //return; + } + 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_)}); + //return; + } + } + } +} + +bool EnbroadParse::check_sum() { + char checksum=0; + char compare=0; + size_t len = buffer_.size() - 3; + size_t i=0; + while (len--) + { + checksum += *(int8_t*)(buffer_.data()+i); + i++; + } + checksum = ~checksum; + + checksum=checksum|0x30; + compare=*(char*)(buffer_.data() + buffer_.size() - 3); + if(checksum==compare) + { + return true; + } + else + { + return false; + } + +} + +bool EnbroadParse::PrepareMessage() { + if (!check_sum()) { + AWARN << "check sum failed. bad frame ratio"; + 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"; + return false; + } + break; + + case enbroad::BIN_SINS_DATA: + break; + case enbroad::BIN_IMU_DATA: + break; + case enbroad::BIN_GNSS_DATA: + if(!HandleGNSSData(reinterpret_cast(message))) + { + return false; + } + break; + default: + return false; + break; + } + + return true; +} + +bool EnbroadParse::HandleSINSData(const enbroad::NAV_SINS_TypeDef* pSinsData) +{ + return true; +} +bool EnbroadParse::HandleIMUData(const enbroad::NAV_IMU_TypeDef* pImuData) +{ + return true; +} +bool EnbroadParse::HandleGNSSData(const enbroad::NAV_GNSS_TypeDef* pGnssData) +{ + return true; +} +bool EnbroadParse::HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData) +{ + //static float senor_temp; + static unsigned short rtkStatus; + //static unsigned short headingStatus; + //static unsigned short velStatus; + //static unsigned short CanInfoFlag; + static unsigned short Nav_Standard_flag; + //static unsigned short Nav_Status; + static unsigned short Sate_Num; + static float baseline; + float imu_measurement_span = 1.0f / 100.0f; + + double seconds = pNavData->gps_week * SECONDS_PER_WEEK + pNavData->gps_millisecs * 1e-3; + /*********************************ins_****************************************************/ + ins_.set_measurement_time(seconds); + ins_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); + ins_.mutable_euler_angles()->set_x(double(pNavData->roll*Coder_Angle_Scale/Coder_Sensor_Scale* DEG_TO_RAD)); + ins_.mutable_euler_angles()->set_y(double(pNavData->pitch*Coder_Angle_Scale/Coder_Sensor_Scale* DEG_TO_RAD)); + //enbroad 北偏西为正，此处北偏东为正 + ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(-double(pNavData->head*Coder_Angle_Scale/Coder_Sensor_Scale))); + ins_.mutable_position()->set_lon(double(pNavData->lon/Coder_Pos_Scale)); + ins_.mutable_position()->set_lat(double(pNavData->lat/Coder_Pos_Scale)); + ins_.mutable_position()->set_height(double(pNavData->alt/1000.0)); + ins_.mutable_linear_acceleration()->set_x(double(pNavData->accX*Coder_Accel_Scale/Coder_Sensor_Scale)); + ins_.mutable_linear_acceleration()->set_y(double(pNavData->accY*Coder_Accel_Scale/Coder_Sensor_Scale)); + ins_.mutable_linear_acceleration()->set_z(double(pNavData->accZ*Coder_Accel_Scale/Coder_Sensor_Scale)); + ins_.mutable_angular_velocity()->set_x(double(pNavData->gyroX*Coder_Rate_Scale/Coder_Sensor_Scale)); + ins_.mutable_angular_velocity()->set_y(double(pNavData->gyroY*Coder_Rate_Scale/Coder_Sensor_Scale)); + ins_.mutable_angular_velocity()->set_z(double(pNavData->gyroZ*Coder_Rate_Scale/Coder_Sensor_Scale)); + ins_.mutable_linear_velocity()->set_x(double(pNavData->ve*Coder_Vel_Scale/Coder_Sensor_Scale)); + ins_.mutable_linear_velocity()->set_y(double(pNavData->vn*Coder_Vel_Scale/Coder_Sensor_Scale)); + ins_.mutable_linear_velocity()->set_z(double(pNavData->vu*Coder_Vel_Scale/Coder_Sensor_Scale)); + ins_.set_type(Ins::GOOD); + + switch(pNavData->poll_type) + { + case enbroad::E_POLL_DEV_TEMP: + //senor_temp = pNavData->poll_frame1/Coder_Sensor_Scale*Coder_Temp_Scale; + break; + case enbroad::E_POLL_GNSS_STATE: + rtkStatus = pNavData->poll_frame1; + //headingStatus = pNavData->poll_frame2; + //velStatus = pNavData->poll_frame3; + break; + case enbroad::E_POLL_CAN_STATE: + //CanInfoFlag = pNavData->poll_frame1; + break; + case enbroad::E_POLL_INS_STATE: + Nav_Standard_flag= pNavData->poll_frame1; + //Nav_Status = pNavData->poll_frame2; + break; + case enbroad::E_POLL_GNSS2_STATE: + Sate_Num = pNavData->poll_frame1; + baseline = pNavData->poll_frame2/1000.0; + break; + } + + /*********************************imu_****************************************************/ + imu_.set_measurement_time(seconds); + imu_.set_measurement_span(imu_measurement_span); + imu_.mutable_linear_acceleration()->set_x(double(pNavData->accX*Coder_Accel_Scale/Coder_Sensor_Scale)); + imu_.mutable_linear_acceleration()->set_y(double(pNavData->accY*Coder_Accel_Scale/Coder_Sensor_Scale)); + imu_.mutable_linear_acceleration()->set_z(double(pNavData->accZ*Coder_Accel_Scale/Coder_Sensor_Scale)); + imu_.mutable_angular_velocity()->set_x(double(pNavData->gyroX*Coder_Rate_Scale/Coder_Sensor_Scale)); + imu_.mutable_angular_velocity()->set_y(double(pNavData->gyroY*Coder_Rate_Scale/Coder_Sensor_Scale)); + imu_.mutable_angular_velocity()->set_z(double(pNavData->gyroZ*Coder_Rate_Scale/Coder_Sensor_Scale)); + + /*********************************bestpos_****************************************************/ + bestpos_.set_measurement_time(seconds); + bestpos_.set_longitude(double(pNavData->lon/Coder_Pos_Scale)); + bestpos_.set_latitude(double(pNavData->lat/Coder_Pos_Scale)); + bestpos_.set_height_msl(double(pNavData->alt/1000.0)); + + + bestpos_.set_undulation(0.0);//undulation = height_wgs84 - height_msl + bestpos_.set_datum_id(static_cast(enbroad::DatumId::WGS84));//datum id number.WGS84 + bestpos_.set_latitude_std_dev(0.0); + bestpos_.set_longitude_std_dev(0.0); + bestpos_.set_height_std_dev(0.0); + bestpos_.set_base_station_id("0");//base station id + bestpos_.set_solution_age(0.0);//solution age (sec) + bestpos_.set_num_sats_tracked(Sate_Num);//number of satellites tracked + bestpos_.set_num_sats_in_solution(Sate_Num);//number of satellites used in solution + bestpos_.set_num_sats_in_solution(Sate_Num);//number of L1/E1/B1 satellites used in solution + bestpos_.set_num_sats_multi(Sate_Num);//number of multi-frequency satellites used in solution + bestpos_.set_extended_solution_status(SolutionType::INS_RTKFIXED);//extended solution status - OEMV and + bestpos_.set_galileo_beidou_used_mask(0); + bestpos_.set_gps_glonass_used_mask(0); + + /*********************************heading_****************************************************/ + heading_.set_measurement_time(seconds); + heading_.set_pitch(double(pNavData->pitch*Coder_Angle_Scale/Coder_Sensor_Scale)); + //enbroad 北偏西为正，此处北偏东为正 + heading_.set_heading(double(-pNavData->head*Coder_Angle_Scale/Coder_Sensor_Scale)); + + heading_.set_baseline_length(baseline); + heading_.set_reserved(0); + heading_.set_heading_std_dev(0.0); + heading_.set_pitch_std_dev(0.0); + heading_.set_station_id("0"); + heading_.set_satellite_tracked_number(Sate_Num);//number of satellites tracked + heading_.set_satellite_soulution_number(Sate_Num);//number of satellites used in solution + heading_.set_satellite_number_obs(Sate_Num);//number of L1/E1/B1 satellites used in solution + heading_.set_satellite_number_multi(Sate_Num);//number of multi-frequency satellites used in solution + 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); + + /*********************************ins_stat_****************************************************/ + ins_stat_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); + //根据RTK状态， 固定为INS_RTKFIXED 浮点为INS_RTKFLOAT，单点及伪距差分SINGLE 无解：NONE + if(enbroad::E_GPS_RTK_FIXED == rtkStatus) + { + ins_stat_.set_pos_type(SolutionType::INS_RTKFIXED); + bestpos_.set_sol_type(SolutionType::INS_RTKFIXED); + heading_.set_position_type(SolutionType::INS_RTKFIXED); + } + else if(enbroad::E_GPS_RTK_FLOAT == rtkStatus) + { + ins_stat_.set_pos_type(SolutionType::INS_RTKFLOAT); + bestpos_.set_sol_type(SolutionType::INS_RTKFLOAT); + heading_.set_position_type(SolutionType::INS_RTKFIXED); + } + else if(enbroad::E_GPS_RTK_SPP == rtkStatus || enbroad::E_GPS_RTK_DGPS == rtkStatus) + { + ins_stat_.set_pos_type(SolutionType::SINGLE); + bestpos_.set_sol_type(SolutionType::SINGLE); + heading_.set_position_type(SolutionType::SINGLE); + } + else + { + ins_stat_.set_pos_type(SolutionType::NONE); + bestpos_.set_sol_type(SolutionType::NONE); + heading_.set_position_type(SolutionType::NONE); + } + + //根据组合导航标定状态：1. 未标定SOL_COMPUTED 2. 标定中SOL_COMPUTED 3. 完成标定SOL_COMPUTED + if(enbroad::E_NAV_STANDARD_PROCCSSED == Nav_Standard_flag) + { + ins_stat_.set_ins_status(SolutionStatus::SOL_COMPUTED); + bestpos_.set_sol_status(SolutionStatus::SOL_COMPUTED); + heading_.set_solution_status(SolutionStatus::SOL_COMPUTED); + } + else if(enbroad::E_NAV_STANDARD_PROCCSSING == Nav_Standard_flag) + { + ins_stat_.set_ins_status(SolutionStatus::COLD_START); + bestpos_.set_sol_status(SolutionStatus::COLD_START); + heading_.set_solution_status(SolutionStatus::SOL_COMPUTED); + } + else + { + ins_stat_.set_ins_status(SolutionStatus::INSUFFICIENT_OBS); + bestpos_.set_sol_status(SolutionStatus::INSUFFICIENT_OBS); + heading_.set_solution_status(SolutionStatus::SOL_COMPUTED); + } + + + return true; +} + +} // namespace gnss +} // namespace drivers +} // namespace apollo + +#endif + diff --git a/modules/drivers/gnss/parser/parser.h b/modules/drivers/gnss/parser/parser.h index 6041c74fa54..8a1215ddf6a 100644 --- a/modules/drivers/gnss/parser/parser.h +++ b/modules/drivers/gnss/parser/parser.h @@ -82,6 +82,7 @@ class Parser { static Parser *CreateHuaCeText(const config::Config &config); static Parser *CreateAsensing(const config::Config &config); static Parser *CreateBroadGnssText(const config::Config &config); + static Parser *CreateEnbroad(const config::Config &config); static Parser *CreateParser(const config::Config &config) { switch (config.data().format()) { @@ -93,6 +94,8 @@ class Parser { return Parser::CreateAsensing(config); case config::Stream::BROADGNSS_TEXT: return Parser::CreateBroadGnssText(config); + case config::Stream::ENBROAD_BINARY: + return Parser::CreateEnbroad(config); default: return nullptr; } diff --git a/modules/drivers/gnss/proto/config.proto b/modules/drivers/gnss/proto/config.proto index fbc6de8aad8..dd2299d7f77 100644 --- a/modules/drivers/gnss/proto/config.proto +++ b/modules/drivers/gnss/proto/config.proto @@ -25,6 +25,10 @@ message Stream { BROADGNSS_TEXT = 30; BROADGNSS_BINARY = 31; + + ENBROAD_TEXT = 40; + ENBROAD_BINARY = 41; + } optional Format format = 1; From 54968ea06f164092ba7976b8d55e73bdd9c824b2 Mon Sep 17 00:00:00 2001 From: dengwei19840721 Date: Tue, 7 May 2024 20:21:45 +0800 Subject: [PATCH 2/5] feat:modify enbroad gnss driver --- modules/drivers/gnss/conf/gnss_conf.pb.txt | 18 +- .../parser/enbroad_parser/enbroad_messages.h | 163 ++++++++------ .../parser/enbroad_parser/enbroad_parser.cc | 206 ++++++++++++++++-- 3 files changed, 290 insertions(+), 97 deletions(-) diff --git a/modules/drivers/gnss/conf/gnss_conf.pb.txt b/modules/drivers/gnss/conf/gnss_conf.pb.txt index 949d7e6626e..40c0c7c22f6 100644 --- a/modules/drivers/gnss/conf/gnss_conf.pb.txt +++ b/modules/drivers/gnss/conf/gnss_conf.pb.txt @@ -1,11 +1,11 @@ data { # format optional: NOVATEL_BINARY, HUACE_TEXT, ASENSING_BINARY, BROADGNSS_TEXT,ENBROAD_BINARY - format: ENBROAD_BINARY + format: NOVATEL_BINARY # protocol optional: serial, tcp, udp, ntrip, can_card_parameter serial { - device: "/dev/ttyUSB0" - baud_rate: 460800 + device: "/dev/novatel0" + baud_rate: 115200 } # tcp, udp: e.g. @@ -37,18 +37,18 @@ data { #} rtk_to { - format: ENBROAD_BINARY + format: NOVATEL_BINARY serial { - device: "/dev/ttyUSB0" - baud_rate: 460800 + device: "/dev/novatel1" + baud_rate: 115200 } } command { - format: ENBROAD_BINARY + format: NOVATEL_BINARY serial { - device: "/dev/ttyUSB0" - baud_rate: 460800 + device: "/dev/novatel2" + baud_rate: 115200 } } diff --git a/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h b/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h index 56c3897d5f3..fa87fb9514b 100755 --- a/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h +++ b/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h @@ -31,6 +31,16 @@ namespace drivers { namespace gnss { namespace enbroad { +//融合定位状态 +enum EFUSHIONSTATUS +{ + E_FUNSION_NONE=0, //无效融合 + E_FUNSION_GPS=1, //GPS融合 + E_FUNSION_WHEEL=2, //用wheel融合 + E_FUNSION_MOTION=3, //运动模型约束 + E_FUNSION_TOTAL=4, +}; + enum ENAVSTANDARDFLAGSTATUS { E_NAV_STANDARD_NO_PROCCSS=0, //0:未标定完成 @@ -123,90 +133,111 @@ struct NAV_DATA_TypeDef{ uint32_t gps_week; // GPS Week number. uint32_t gps_millisecs; // Milliseconds of week. - uint16_t pitch; - uint16_t roll; - uint16_t head; - uint16_t gyroX; - uint16_t gyroY; - uint16_t gyroZ; - uint16_t accX; - uint16_t accY; - uint16_t accZ; - uint16_t magnetX; - uint16_t magnetY; - uint16_t magentZ; + int16_t pitch; + int16_t roll; + int16_t head; + int16_t gyroX; + int16_t gyroY; + int16_t gyroZ; + int16_t accX; + int16_t accY; + int16_t accZ; + int16_t magnetX; + int16_t magnetY; + int16_t magentZ; long long lat; long long lon; - uint32_t alt; - uint16_t ve; - uint16_t vn; - uint16_t vu; + int32_t alt; + int16_t ve; + int16_t vn; + int16_t vu; uint8_t poll_type; - uint16_t poll_frame1; - uint16_t poll_frame2; - uint16_t poll_frame3; + int16_t poll_frame1; + int16_t poll_frame2; + int16_t poll_frame3; }; static_assert(sizeof(NAV_DATA_TypeDef) == 65, "Incorrect size of NAV_DATA_TypeDef"); struct NAV_SINS_TypeDef{ - uint32_t gps_week; // GPS Week number. - uint32_t gps_millisecs; // Milliseconds of week. - - double pitch; - double roll; - double heading; - double ve; //m/s - double vn; - double vu; - double latitude; //degree - double longitude; - double altitude; - + uint32_t gps_week; // GPS Week number. + uint32_t gpssecond; // Milliseconds of week. + uint8_t navStatus; //0:START;1:ROUTH_ALIGN;2:SINS_KF_INITIAL;3:SYSTEM_STANDARD;4:IN_NAV;5:stop + uint8_t fusion; //0:valid;1:gps;2:wheel;3:motion + float pitch; //unit degree + float roll; //unit degree + float heading; //unit degree + float ve; //East velocity, unit m/s + float vn; //North velocity,unit m/s + float vu; //up velocity,unit m/s + double latitude; //unit degree + double longitude; //unit degree + float altitude; //unit m + float xigema_ve; //East velocity std unit m/s + float xigema_vn; //North velocity std unit m/s + float xigema_vu; //up velocity std unit m/s + 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_pitch; //pitch std unit degree + float xigema_roll; //roll std unit degree + float xigema_head; //heading std unit degree }; -static_assert(sizeof(NAV_SINS_TypeDef) == 80, "Incorrect size of NAV_SINS_TypeDef"); +static_assert(sizeof(NAV_SINS_TypeDef) == 90, "Incorrect size of NAV_SINS_TypeDef"); struct NAV_IMU_TypeDef{ - uint32_t gps_week; // GPS Week number. - uint32_t gps_millisecs; // Milliseconds of week. - - uint8_t gyroFlag; //角速度计状态0：异常，1：正常 - double gyroX; //degree/s - double gyroY; - double gyroZ; - - uint8_t accFlag; //0：异常，1：正常 - double accX; //m/s2 - double accY; - double accZ; - - uint8_t magnetFlag; //0：异常，1：正常 - double magnetX; //mGauss - double magnetY; - double magnetZ; + uint32_t gps_week; // GPS Week number. + uint32_t gpssecond; // Milliseconds of week. + float sensorTemp; //unit degree + + uint8_t gyroFlag; //gyro state,0:abnormal,1:normal + double gyroX; //unit degree/s + double gyroY; //unit degree/s + double gyroZ; //unit degree/s + + uint8_t accFlag; //acc state,0:abnormal,1:normal + double accX; //unit m/s2 + double accY; //unit m/s2 + double accZ; //unit m/s2 + + uint8_t magnetFlag; //magnet state,0:abnormal,1:normal + double magnetX; //unit mGauss + double magnetY; //unit mGauss + double magnetZ; //unit mGauss }; -static_assert(sizeof(NAV_IMU_TypeDef) == 83, "Incorrect size of NAV_IMU_TypeDef"); +static_assert(sizeof(NAV_IMU_TypeDef) == 87, "Incorrect size of NAV_IMU_TypeDef"); struct NAV_GNSS_TypeDef{ - uint32_t gps_week; // GPS Week number. - uint32_t gps_millisecs; // Milliseconds of week. + uint32_t gps_week; // GPS Week number. + uint32_t gpssecond; // Milliseconds of week. + uint8_t satsNum; //num of sats + float age; // - uint8_t rtkStatus; // - double latitude; //degree - double longitude; - double altitude; - - uint8_t headingStatus; - double heading; - - uint8_t velStatus; - double ve; //m/s - double vn; - double vu; + uint8_t rtkStatus; //0:invalid;1:SPP;2:DGPS;4:FIXED;5:FLOAT + double latitude; //unit degree + double longitude; //unit degree + float altitude; //unit m + + uint8_t headingStatus; //0:invalid;4:FIXED;5:FLOAT + float baseline; //unit m + float heading; //unit degree + + uint8_t velStatus; //0:normal + float ve; //East velocity, unit m/s + float vn; //North velocity,unit m/s + float vu; //up velocity,unit m/s + + float xigema_ve; //East velocity std unit m/s + float xigema_vn; //North velocity std unit m/s + float xigema_vu; //up velocity std unit m/s + float xigema_lat; //North pos std unit m + float xigema_lon; //East pos std unit m + float xigema_alt; //up pos std unit m + }; -static_assert(sizeof(NAV_GNSS_TypeDef) == 67, "Incorrect size of NAV_GNSS_TypeDef"); +static_assert(sizeof(NAV_GNSS_TypeDef) == 80, "Incorrect size of NAV_GNSS_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 0fecdac1858..ade4e7dbf3b 100755 --- a/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc +++ b/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc @@ -174,6 +174,15 @@ void EnbroadParse::GetMessages(MessageInfoVec *messages) } } } +double normalizeAngleTo180(double angle) { + while (angle > 180.0) { + angle -= 360.0; + } + while (angle <= -180.0) { + angle += 360.0; + } + return angle; +} bool EnbroadParse::check_sum() { char checksum=0; @@ -201,8 +210,12 @@ bool EnbroadParse::check_sum() { } bool EnbroadParse::PrepareMessage() { + static long long sumframe=0; + static long long badframe=0; + sumframe++; if (!check_sum()) { - AWARN << "check sum failed. bad frame ratio"; + badframe++; + AERROR << "check sum failed. bad frame ratio"<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"; - return false; - } - break; + 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"; + return false; + } - case enbroad::BIN_SINS_DATA: + 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"; + return false; + } 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"; + return false; + } 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; - default: - return false; - break; + default: + return false; + break; } return true; @@ -246,14 +285,137 @@ bool EnbroadParse::PrepareMessage() { bool EnbroadParse::HandleSINSData(const enbroad::NAV_SINS_TypeDef* pSinsData) { + float imu_measurement_span = 1.0f / 100.0f; + double seconds = pSinsData->gps_week * SECONDS_PER_WEEK + pSinsData->gpssecond * 1e-3; + /*********************************ins_****************************************************/ + ins_.set_measurement_time(seconds); + ins_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); + ins_.mutable_euler_angles()->set_x(pSinsData->roll* DEG_TO_RAD); + ins_.mutable_euler_angles()->set_y(pSinsData->pitch* DEG_TO_RAD); + //enbroad 北偏西为正，此处北偏东为正 + ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(normalizeAngleTo180(-pSinsData->heading))); + ins_.mutable_position()->set_lon(pSinsData->longitude); + ins_.mutable_position()->set_lat(pSinsData->latitude); + ins_.mutable_position()->set_height(pSinsData->altitude); + 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) + { + ins_.set_type(Ins::GOOD); + } + else if(enbroad::E_NAV_STATUS_SYSTEM_STANDARD ==pSinsData->navStatus) + { + ins_.set_type(Ins::CONVERGING); + } + else + { + ins_.set_type(Ins::INVALID); + } + /*********************************bestpos_****************************************************/ + 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 + bestpos_.set_datum_id(static_cast(enbroad::DatumId::WGS84));//datum id number.WGS84 + //标准差填写组合导航的 + 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_****************************************************/ + ins_stat_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); + //ins位置类型，融合GPS则INS_RTKFIXED，融合wheel为INS_RTKFLOAT,融合motion为SINGLE，无需为NONE + if(enbroad::E_FUNSION_GPS ==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) + { + 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) + { + 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) { + float imu_measurement_span = 1.0f / 100.0f; + double seconds = pImuData->gps_week * SECONDS_PER_WEEK + pImuData->gpssecond * 1e-3; + /*********************************imu_****************************************************/ + imu_.set_measurement_time(seconds); + imu_.set_measurement_span(imu_measurement_span); + imu_.mutable_linear_acceleration()->set_x(pImuData->accX); + imu_.mutable_linear_acceleration()->set_y(pImuData->accY); + imu_.mutable_linear_acceleration()->set_z(pImuData->accZ); + imu_.mutable_angular_velocity()->set_x(pImuData->gyroX); + imu_.mutable_angular_velocity()->set_y(pImuData->gyroY); + imu_.mutable_angular_velocity()->set_z(pImuData->gyroZ); + return true; } bool EnbroadParse::HandleGNSSData(const enbroad::NAV_GNSS_TypeDef* pGnssData) { + //填充bestpos中有关gnss数据及heading所有数据 + double seconds = pGnssData->gps_week * SECONDS_PER_WEEK + pGnssData->gpssecond * 1e-3; + //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);//number of satellites tracked + bestpos_.set_num_sats_in_solution(pGnssData->satsNum);//number of satellites used in solution + bestpos_.set_num_sats_in_solution(pGnssData->satsNum);//number of L1/E1/B1 satellites used in solution + bestpos_.set_num_sats_multi(pGnssData->satsNum);//number of multi-frequency satellites used in solution + //bestpos_.set_galileo_beidou_used_mask(0); + //bestpos_.set_gps_glonass_used_mask(0); + /*********************************heading_****************************************************/ + heading_.set_measurement_time(seconds); + //gnss航向 + heading_.set_heading(pGnssData->heading); + heading_.set_baseline_length(pGnssData->baseline); + heading_.set_reserved(0); + //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);//number of satellites tracked + heading_.set_satellite_soulution_number(pGnssData->satsNum);//number of satellites used in solution + heading_.set_satellite_number_obs(pGnssData->satsNum);//number of L1/E1/B1 satellites used in solution + heading_.set_satellite_number_multi(pGnssData->satsNum);//number of multi-frequency satellites used in solution + //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); + } return true; } bool EnbroadParse::HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData) @@ -276,7 +438,7 @@ bool EnbroadParse::HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData) ins_.mutable_euler_angles()->set_x(double(pNavData->roll*Coder_Angle_Scale/Coder_Sensor_Scale* DEG_TO_RAD)); ins_.mutable_euler_angles()->set_y(double(pNavData->pitch*Coder_Angle_Scale/Coder_Sensor_Scale* DEG_TO_RAD)); //enbroad 北偏西为正，此处北偏东为正 - ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(-double(pNavData->head*Coder_Angle_Scale/Coder_Sensor_Scale))); + ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(normalizeAngleTo180(-pNavData->head*Coder_Angle_Scale/Coder_Sensor_Scale))); ins_.mutable_position()->set_lon(double(pNavData->lon/Coder_Pos_Scale)); ins_.mutable_position()->set_lat(double(pNavData->lat/Coder_Pos_Scale)); ins_.mutable_position()->set_height(double(pNavData->alt/1000.0)); @@ -350,8 +512,8 @@ bool EnbroadParse::HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData) heading_.set_measurement_time(seconds); heading_.set_pitch(double(pNavData->pitch*Coder_Angle_Scale/Coder_Sensor_Scale)); //enbroad 北偏西为正，此处北偏东为正 - heading_.set_heading(double(-pNavData->head*Coder_Angle_Scale/Coder_Sensor_Scale)); - + heading_.set_heading(normalizeAngleTo180(-pNavData->head*Coder_Angle_Scale/Coder_Sensor_Scale)); + //AERROR << "heading_.set_heading: " << normalizeAngleTo180(-pNavData->head*Coder_Angle_Scale/Coder_Sensor_Scale); heading_.set_baseline_length(baseline); heading_.set_reserved(0); heading_.set_heading_std_dev(0.0); @@ -379,7 +541,7 @@ bool EnbroadParse::HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData) { ins_stat_.set_pos_type(SolutionType::INS_RTKFLOAT); bestpos_.set_sol_type(SolutionType::INS_RTKFLOAT); - heading_.set_position_type(SolutionType::INS_RTKFIXED); + heading_.set_position_type(SolutionType::INS_RTKFLOAT); } else if(enbroad::E_GPS_RTK_SPP == rtkStatus || enbroad::E_GPS_RTK_DGPS == rtkStatus) { @@ -405,13 +567,13 @@ bool EnbroadParse::HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData) { ins_stat_.set_ins_status(SolutionStatus::COLD_START); bestpos_.set_sol_status(SolutionStatus::COLD_START); - heading_.set_solution_status(SolutionStatus::SOL_COMPUTED); + heading_.set_solution_status(SolutionStatus::COLD_START); } else { ins_stat_.set_ins_status(SolutionStatus::INSUFFICIENT_OBS); bestpos_.set_sol_status(SolutionStatus::INSUFFICIENT_OBS); - heading_.set_solution_status(SolutionStatus::SOL_COMPUTED); + heading_.set_solution_status(SolutionStatus::INSUFFICIENT_OBS); } From 9cf32ab9492114afd57cc1f9f055f65713a724e3 Mon Sep 17 00:00:00 2001 From: dengwei19840721 Date: Thu, 23 May 2024 10:50:45 +0800 Subject: [PATCH 3/5] feat:modify enbroad gnss driver and improve compatibility --- .../parser/enbroad_parser/enbroad_messages.h | 281 +++--- .../parser/enbroad_parser/enbroad_parser.cc | 848 ++++++++---------- 2 files changed, 519 insertions(+), 610 deletions(-) diff --git a/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h b/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h index fa87fb9514b..e481607b0c1 100755 --- a/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h +++ b/modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h @@ -18,7 +18,6 @@ // refer to ENS's // documents for details about these messages. - #pragma once #include @@ -31,69 +30,59 @@ namespace drivers { namespace gnss { namespace enbroad { -//融合定位状态 -enum EFUSHIONSTATUS -{ - E_FUNSION_NONE=0, //无效融合 - E_FUNSION_GPS=1, //GPS融合 - E_FUNSION_WHEEL=2, //用wheel融合 - E_FUNSION_MOTION=3, //运动模型约束 - E_FUNSION_TOTAL=4, +enum EFUSHIONSTATUS { + E_FUNSION_NONE = 0, + E_FUNSION_GPS = 1, + E_FUNSION_WHEEL = 2, + E_FUNSION_MOTION = 3, + E_FUNSION_TOTAL = 4, }; -enum ENAVSTANDARDFLAGSTATUS -{ - E_NAV_STANDARD_NO_PROCCSS=0, //0:未标定完成 - E_NAV_STANDARD_PROCCSSING=1, //1：标定中 - E_NAV_STANDARD_PROCCSSED=2, //2：标定完成 - E_NAV_STANDARD_TOTAL=3, //总共状态数目 +enum ENAVSTANDARDFLAGSTATUS { + E_NAV_STANDARD_NO_PROCCSS = 0, + E_NAV_STANDARD_PROCCSSING = 1, + E_NAV_STANDARD_PROCCSSED = 2, + E_NAV_STANDARD_TOTAL = 3, }; -enum ENAVSTATUS -{ - E_NAV_STATUS_START =0, //开始导航加载导航参数 - E_NAV_STATUS_ROUTH_ALIGN=1, //SINS初对准 - E_NAV_STATUS_SINS_KF_INITIAL=2, //SINS、KF初始化 - E_NAV_STATUS_SYSTEM_STANDARD=3, //系统标定 - E_NAV_STATUS_IN_NAV=4, //正常导航 - E_NAV_STATUS_STOP=5, //停止导航 - E_NAV_STATUS_TOTAL=6, //总共状态数目 +enum ENAVSTATUS { + E_NAV_STATUS_START = 0, + E_NAV_STATUS_ROUTH_ALIGN = 1, + E_NAV_STATUS_SINS_KF_INITIAL = 2, + E_NAV_STATUS_SYSTEM_STANDARD = 3, + E_NAV_STATUS_IN_NAV = 4, + E_NAV_STATUS_STOP = 5, + E_NAV_STATUS_TOTAL = 6, }; -//rtk状态 -enum EGPSRTKSTATUS -{ - E_GPS_RTK_INVALID=0, //0: 无定位 - E_GPS_RTK_SPP=1, //1：单点定位 - E_GPS_RTK_DGPS=2, //2：差分解 - E_GPS_RTK_FIXED=4, //4：RTK固定解 - E_GPS_RTK_FLOAT=5, //5：RTK浮动解 - E_GPS_RTK_TOTAL=6, - +enum EGPSRTKSTATUS { + E_GPS_RTK_INVALID = 0, + E_GPS_RTK_SPP = 1, + E_GPS_RTK_DGPS = 2, + E_GPS_RTK_FIXED = 4, + E_GPS_RTK_FLOAT = 5, + E_GPS_RTK_TOTAL = 6, }; -enum EPOLLDATATYPE -{ - E_POLL_DEV_TEMP =0, - E_POLL_GNSS_STATE=20, - E_POLL_IMU_STATE=40, - E_POLL_CAN_STATE=60, - E_POLL_INS_STATE=80, - E_POLL_GNSS2_STATE=100, - E_POLL_DATA_TOTAL=120, +enum EPOLLDATATYPE { + E_POLL_DEV_TEMP = 0, + E_POLL_GNSS_STATE = 20, + E_POLL_IMU_STATE = 40, + E_POLL_CAN_STATE = 60, + E_POLL_INS_STATE = 80, + E_POLL_GNSS2_STATE = 100, + E_POLL_DATA_TOTAL = 120, }; - enum MessageId : uint16_t { - BIN_NAV_DATA = 0x01AA, + BIN_NAV_DATA = 0x01AA, BIN_SINS_DATA = 0x02AA, - BIN_IMU_DATA = 0x03AA, + BIN_IMU_DATA = 0x03AA, BIN_GNSS_DATA = 0x04AA, }; // Every binary message has 32-bit CRC performed on all data including the // header. -//constexpr uint16_t CRC_LENGTH = 4; #pragma pack(push, 1) // Turn off struct padding. @@ -102,35 +91,30 @@ enum class DatumId : uint32_t { WGS84 = 61, }; - -//帧头 enum SyncHeadByte : uint8_t { SYNC_HEAD_0 = 0xA5, SYNC_HEAD_1 = 0x5A, SYNC_HEAD_2 = 0x5A, }; -//帧尾 + enum SyncTailByte : uint8_t { SYNC_TAIL_0 = 0xA5, SYNC_Tail_1 = 0x5A, SYNC_Tail_2 = 0x5A, }; -//Frame Head -struct FrameHeader{ - SyncHeadByte sync_head[3];//sync Head - uint8_t src_id;//src id - uint8_t dst_id;//dst id - MessageId message_id;//message id - uint8_t rsp; //RSP - uint16_t SN; //session NO. - uint16_t reserved;//RESERVED - uint16_t message_length; // Message length +struct FrameHeader { + SyncHeadByte sync_head[3]; + uint8_t src_id; + uint8_t dst_id; + MessageId message_id; + uint8_t rsp; + uint16_t SN; + uint16_t reserved; + uint16_t message_length; }; - -static_assert(sizeof(FrameHeader) == 14, "Incorrect size of FrameHeader"); -struct NAV_DATA_TypeDef{ - +static_assert(sizeof(FrameHeader) == 14, "Incorrect FrameHeader"); +struct NAV_DATA_TypeDef { uint32_t gps_week; // GPS Week number. uint32_t gps_millisecs; // Milliseconds of week. int16_t pitch; @@ -145,104 +129,91 @@ struct NAV_DATA_TypeDef{ int16_t magnetX; int16_t magnetY; int16_t magentZ; - long long lat; - long long lon; - int32_t alt; - int16_t ve; - int16_t vn; - int16_t vu; - uint8_t poll_type; - int16_t poll_frame1; - int16_t poll_frame2; - int16_t poll_frame3; + int64_t lat; + int64_t lon; + int32_t alt; + int16_t ve; + int16_t vn; + int16_t vu; + uint8_t poll_type; + int16_t poll_frame1; + int16_t poll_frame2; + int16_t poll_frame3; }; -static_assert(sizeof(NAV_DATA_TypeDef) == 65, "Incorrect size of NAV_DATA_TypeDef"); - -struct NAV_SINS_TypeDef{ - - uint32_t gps_week; // GPS Week number. - uint32_t gpssecond; // Milliseconds of week. - uint8_t navStatus; //0:START;1:ROUTH_ALIGN;2:SINS_KF_INITIAL;3:SYSTEM_STANDARD;4:IN_NAV;5:stop - uint8_t fusion; //0:valid;1:gps;2:wheel;3:motion - float pitch; //unit degree - float roll; //unit degree - float heading; //unit degree - float ve; //East velocity, unit m/s - float vn; //North velocity,unit m/s - float vu; //up velocity,unit m/s - double latitude; //unit degree - double longitude; //unit degree - float altitude; //unit m - float xigema_ve; //East velocity std unit m/s - float xigema_vn; //North velocity std unit m/s - float xigema_vu; //up velocity std unit m/s - 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_pitch; //pitch std unit degree - float xigema_roll; //roll std unit degree - float xigema_head; //heading std unit degree +static_assert(sizeof(NAV_DATA_TypeDef) == 65, "Incorrect NAV_DATA_TypeDef"); + +struct NAV_SINS_TypeDef { + uint32_t gps_week; // GPS Week number. + uint32_t gpssecond; // Milliseconds of week. + uint8_t navStatus; + uint8_t fusion; // 0:valid;1:gps;2:wheel;3:motion + float pitch; + float roll; // unit degree + float heading; // unit degree + float ve; // East velocity, unit m/s + float vn; // North velocity,unit m/s + float vu; // up velocity,unit m/s + double latitude; // unit degree + double longitude; // unit degree + float altitude; // unit m + float xigema_ve; // East velocity std unit m/s + float xigema_vn; // North velocity std unit m/s + float xigema_vu; // up velocity std unit m/s + 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_pitch; // pitch std unit degree + float xigema_roll; // roll std unit degree + float xigema_head; // heading std unit degree }; -static_assert(sizeof(NAV_SINS_TypeDef) == 90, "Incorrect size of NAV_SINS_TypeDef"); - -struct NAV_IMU_TypeDef{ - - uint32_t gps_week; // GPS Week number. - uint32_t gpssecond; // Milliseconds of week. - float sensorTemp; //unit degree - - uint8_t gyroFlag; //gyro state,0:abnormal,1:normal - double gyroX; //unit degree/s - double gyroY; //unit degree/s - double gyroZ; //unit degree/s - - uint8_t accFlag; //acc state,0:abnormal,1:normal - double accX; //unit m/s2 - double accY; //unit m/s2 - double accZ; //unit m/s2 - - uint8_t magnetFlag; //magnet state,0:abnormal,1:normal - double magnetX; //unit mGauss - double magnetY; //unit mGauss - double magnetZ; //unit mGauss +static_assert(sizeof(NAV_SINS_TypeDef) == 90, "Incorrect NAV_SINS_TypeDef"); + +struct NAV_IMU_TypeDef { + uint32_t gps_week; // GPS Week number. + uint32_t gpssecond; // Milliseconds of week. + float sensorTemp; // unit degree + uint8_t gyroFlag; // gyro state,0:abnormal,1:normal + double gyroX; // unit degree/s + double gyroY; // unit degree/s + double gyroZ; // unit degree/s + uint8_t accFlag; // acc state,0:abnormal,1:normal + double accX; // unit m/s2 + double accY; // unit m/s2 + double accZ; // unit m/s2 + uint8_t magnetFlag; // magnet state,0:abnormal,1:normal + double magnetX; // unit mGauss + double magnetY; // unit mGauss + double magnetZ; // unit mGauss }; -static_assert(sizeof(NAV_IMU_TypeDef) == 87, "Incorrect size of NAV_IMU_TypeDef"); - -struct NAV_GNSS_TypeDef{ - - uint32_t gps_week; // GPS Week number. - uint32_t gpssecond; // Milliseconds of week. - uint8_t satsNum; //num of sats - float age; // - - uint8_t rtkStatus; //0:invalid;1:SPP;2:DGPS;4:FIXED;5:FLOAT - double latitude; //unit degree - double longitude; //unit degree - float altitude; //unit m - - uint8_t headingStatus; //0:invalid;4:FIXED;5:FLOAT - float baseline; //unit m - float heading; //unit degree - - uint8_t velStatus; //0:normal - float ve; //East velocity, unit m/s - float vn; //North velocity,unit m/s - float vu; //up velocity,unit m/s - - float xigema_ve; //East velocity std unit m/s - float xigema_vn; //North velocity std unit m/s - float xigema_vu; //up velocity std unit m/s - float xigema_lat; //North pos std unit m - float xigema_lon; //East pos std unit m - float xigema_alt; //up pos std unit m - +static_assert(sizeof(NAV_IMU_TypeDef) == 87, "Incorrect NAV_IMU_TypeDef"); + +struct NAV_GNSS_TypeDef { + uint32_t gps_week; // GPS Week number. + uint32_t gpssecond; // Milliseconds of week. + uint8_t satsNum; // num of sats + float age; + uint8_t rtkStatus; // 0:invalid;1:SPP;2:DGPS;4:FIXED;5:FLOAT + double latitude; // unit degree + double longitude; // unit degree + float altitude; // unit m + uint8_t headingStatus; // 0:invalid;4:FIXED;5:FLOAT + float baseline; // unit m + float heading; // unit degree + uint8_t velStatus; // 0:normal + float ve; // East velocity, unit m/s + float vn; // North velocity,unit m/s + float vu; // up velocity,unit m/s + float xigema_ve; // East velocity std unit m/s + float xigema_vn; // North velocity std unit m/s + float xigema_vu; // up velocity std unit m/s + float xigema_lat; // North pos std unit m + float xigema_lon; // East pos std unit m + float xigema_alt; // up pos std unit m }; -static_assert(sizeof(NAV_GNSS_TypeDef) == 80, "Incorrect size of NAV_GNSS_TypeDef"); - +static_assert(sizeof(NAV_GNSS_TypeDef) == 80, "Incorrect NAV_GNSS_TypeDef"); #pragma pack(pop) - } // namespace enbroad } // namespace gnss } // namespace drivers diff --git a/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc b/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc index ade4e7dbf3b..80d9e2f4aa9 100755 --- a/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc +++ b/modules/drivers/gnss/parser/enbroad_parser/enbroad_parser.cc @@ -13,7 +13,6 @@ * See the License for the specific language governing permissions and * limitations under the License. *****************************************************************************/ - // An parser for decoding binary messages from a ENS receiver. The following // messages must be // logged in order for this parser to work properly. @@ -36,26 +35,22 @@ #include "modules/drivers/gnss/parser/parser.h" #include "modules/drivers/gnss/parser/parser_common.h" - -#if 1 - namespace apollo { namespace drivers { namespace gnss { - -//编解码协议比例因子设置 -#define Coder_Accel_Scale 12.0 //加计比例因子 -#define Coder_Rate_Scale 300.0 //陀螺比列因子 -#define Coder_MAG_Scale 1.0 //磁力计比列因子，待定 -#define Coder_IFof_Rate_Scale 600.0 -#define Coder_Angle_Scale 360.0 //俯仰、横滚、航向角度比列因子 -#define Coder_Temp_Scale 200.0 //温度比列因子 -#define Coder_Sensor_Scale 32768.0 //传感器比列因子 -#define Coder_IFof_Sensor_Scale 2147483648.0 -#define Coder_EXP_E 2.718282.0 -#define Coder_Vel_Scale 100.0 //速度比列因子 -#define Coder_Pos_Scale 10000000000.0 //经纬高程比例因子 +// Encoding and decoding protocol scaling factor setting +#define Coder_Accel_Scale 12.0 +#define Coder_Rate_Scale 300.0 +#define Coder_MAG_Scale 1.0 +#define Coder_IFof_Rate_Scale 600.0 +#define Coder_Angle_Scale 360.0 +#define Coder_Temp_Scale 200.0 +#define Coder_Sensor_Scale 32768.0 +#define Coder_IFof_Sensor_Scale 2147483648.0 +#define Coder_EXP_E 2.718282.0 +#define Coder_Vel_Scale 100.0 +#define Coder_Pos_Scale 10000000000.0 constexpr size_t BUFFER_SIZE = 256; @@ -64,7 +59,7 @@ class EnbroadParse : public Parser { EnbroadParse(); explicit EnbroadParse(const config::Config& config); - virtual void GetMessages(MessageInfoVec *messages); + virtual void GetMessages(MessageInfoVec* messages); private: bool PrepareMessage(); @@ -74,8 +69,6 @@ class EnbroadParse : public Parser { bool HandleIMUData(const enbroad::NAV_IMU_TypeDef* pImuData); bool HandleGNSSData(const enbroad::NAV_GNSS_TypeDef* pGnssData); - - size_t header_length_ = 0; size_t total_length_ = 0; std::vector buffer_; @@ -87,35 +80,21 @@ class EnbroadParse : public Parser { InsStat ins_stat_; }; -Parser* Parser::CreateEnbroad(const config::Config& config) { +Parser* Parser::CreateEnbroad(const config::Config& config) { return new EnbroadParse(config); } -EnbroadParse::EnbroadParse() { - buffer_.reserve(BUFFER_SIZE); - //ins_.mutable_position_covariance()->Resize(9, FLOAT_NAN); - //ins_.mutable_euler_angles_covariance()->Resize(9, FLOAT_NAN); - //ins_.mutable_linear_velocity_covariance()->Resize(9, FLOAT_NAN); - -} +EnbroadParse::EnbroadParse() { buffer_.reserve(BUFFER_SIZE); } -EnbroadParse::EnbroadParse(const config::Config& config) { +EnbroadParse::EnbroadParse(const config::Config& config) { buffer_.reserve(BUFFER_SIZE); - //ins_.mutable_position_covariance()->Resize(9, FLOAT_NAN); - //ins_.mutable_euler_angles_covariance()->Resize(9, FLOAT_NAN); - //ins_.mutable_linear_velocity_covariance()->Resize(9, FLOAT_NAN); - - //if (config.has_imu_type()) { - // imu_type_ = config.imu_type(); - //} } -void EnbroadParse::GetMessages(MessageInfoVec *messages) -{ - if (data_ == nullptr) { - return; - } - while (data_ < data_end_) { +void EnbroadParse::GetMessages(MessageInfoVec* messages) { + if (data_ == nullptr) { + return; + } + while (data_ < data_end_) { if (buffer_.empty()) { // Looking for SYNC_HEAD_0 if (*data_ == enbroad::SYNC_HEAD_0) { buffer_.push_back(*data_); @@ -127,21 +106,21 @@ void EnbroadParse::GetMessages(MessageInfoVec *messages) } else { buffer_.clear(); } - }else if (buffer_.size() == 2) { // Looking for SYNC_HEAD_2 + } else if (buffer_.size() == 2) { // Looking for SYNC_HEAD_2 if (*data_ == enbroad::SYNC_HEAD_2) { buffer_.push_back(*data_++); - header_length_ = sizeof(enbroad::FrameHeader); + header_length_ = sizeof(enbroad::FrameHeader); } else { buffer_.clear(); } - } - else if (header_length_ > 0) { // Working on header. + } else if (header_length_ > 0) { // Working on header. if (buffer_.size() < header_length_) { buffer_.push_back(*data_++); } else { - - //header_length + datalen + sumcheck + taillength - total_length_ = header_length_ + reinterpret_cast(buffer_.data())->message_length + 1 + 2; + total_length_ = header_length_ + + reinterpret_cast(buffer_.data()) + ->message_length + + 1 + 2; header_length_ = 0; } } else if (total_length_ > 0) { @@ -149,440 +128,399 @@ void EnbroadParse::GetMessages(MessageInfoVec *messages) buffer_.push_back(*data_++); continue; } - if(!PrepareMessage()) - { - buffer_.clear(); - total_length_ = 0; - //return; - } - 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_)}); - //return; - } + if (!PrepareMessage()) { + buffer_.clear(); + total_length_ = 0; + } 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_)}); } } + } } double normalizeAngleTo180(double angle) { - while (angle > 180.0) { - angle -= 360.0; - } - while (angle <= -180.0) { - angle += 360.0; - } - return angle; + while (angle > 180.0) { + angle -= 360.0; + } + while (angle <= -180.0) { + angle += 360.0; + } + return angle; } - bool EnbroadParse::check_sum() { - char checksum=0; - char compare=0; - size_t len = buffer_.size() - 3; - size_t i=0; - while (len--) - { - checksum += *(int8_t*)(buffer_.data()+i); - i++; - } - checksum = ~checksum; - - checksum=checksum|0x30; - compare=*(char*)(buffer_.data() + buffer_.size() - 3); - if(checksum==compare) - { - return true; - } - else - { - return false; - } - + char checksum = 0; + char compare = 0; + size_t len = buffer_.size() - 3; + size_t i = 0; + while (len--) { + checksum += *reinterpret_cast(buffer_.data() + i); + i++; + } + checksum = ~checksum; + checksum = checksum | 0x30; + compare = *reinterpret_cast(buffer_.data() + buffer_.size() - 3); + if (checksum == compare) { + return true; + } else { + return false; + } } -bool EnbroadParse::PrepareMessage() { - static long long sumframe=0; - static long long badframe=0; - sumframe++; +bool EnbroadParse::PrepareMessage() { if (!check_sum()) { - badframe++; - AERROR << "check sum failed. bad frame ratio"<(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"; - return false; - } - - 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"; - return false; - } - 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"; - return false; - } - 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; - default: - return false; - break; + 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"; + return false; + } + 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"; + return false; + } + 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"; + return false; + } + 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; + default: + return false; } - return true; } -bool EnbroadParse::HandleSINSData(const enbroad::NAV_SINS_TypeDef* pSinsData) -{ - float imu_measurement_span = 1.0f / 100.0f; - double seconds = pSinsData->gps_week * SECONDS_PER_WEEK + pSinsData->gpssecond * 1e-3; - /*********************************ins_****************************************************/ - ins_.set_measurement_time(seconds); - ins_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); - ins_.mutable_euler_angles()->set_x(pSinsData->roll* DEG_TO_RAD); - ins_.mutable_euler_angles()->set_y(pSinsData->pitch* DEG_TO_RAD); - //enbroad 北偏西为正，此处北偏东为正 - ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(normalizeAngleTo180(-pSinsData->heading))); - ins_.mutable_position()->set_lon(pSinsData->longitude); - ins_.mutable_position()->set_lat(pSinsData->latitude); - ins_.mutable_position()->set_height(pSinsData->altitude); - 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) - { - ins_.set_type(Ins::GOOD); - } - else if(enbroad::E_NAV_STATUS_SYSTEM_STANDARD ==pSinsData->navStatus) - { - ins_.set_type(Ins::CONVERGING); - } - else - { - ins_.set_type(Ins::INVALID); - } - /*********************************bestpos_****************************************************/ - 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 - bestpos_.set_datum_id(static_cast(enbroad::DatumId::WGS84));//datum id number.WGS84 - //标准差填写组合导航的 - 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_****************************************************/ - ins_stat_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); - //ins位置类型，融合GPS则INS_RTKFIXED，融合wheel为INS_RTKFLOAT,融合motion为SINGLE，无需为NONE - if(enbroad::E_FUNSION_GPS ==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); - } +bool EnbroadParse::HandleSINSData(const enbroad::NAV_SINS_TypeDef* pSinsData) { + double seconds = + pSinsData->gps_week * SECONDS_PER_WEEK + pSinsData->gpssecond * 1e-3; + ins_.set_measurement_time(seconds); + ins_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); + ins_.mutable_euler_angles()->set_x(pSinsData->roll * DEG_TO_RAD); + ins_.mutable_euler_angles()->set_y(pSinsData->pitch * DEG_TO_RAD); + // enbroad set northwest as right direction, Here northeast as right direction + ins_.mutable_euler_angles()->set_z( + azimuth_deg_to_yaw_rad(normalizeAngleTo180(-pSinsData->heading))); + ins_.mutable_position()->set_lon(pSinsData->longitude); + ins_.mutable_position()->set_lat(pSinsData->latitude); + ins_.mutable_position()->set_height(pSinsData->altitude); + 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) { + ins_.set_type(Ins::GOOD); + } else if (enbroad::E_NAV_STATUS_SYSTEM_STANDARD == 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) { + 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) - { - 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) - { - 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; + if (enbroad::E_NAV_STATUS_IN_NAV == 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) { + 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) -{ - float imu_measurement_span = 1.0f / 100.0f; - double seconds = pImuData->gps_week * SECONDS_PER_WEEK + pImuData->gpssecond * 1e-3; - /*********************************imu_****************************************************/ - imu_.set_measurement_time(seconds); - imu_.set_measurement_span(imu_measurement_span); - imu_.mutable_linear_acceleration()->set_x(pImuData->accX); - imu_.mutable_linear_acceleration()->set_y(pImuData->accY); - imu_.mutable_linear_acceleration()->set_z(pImuData->accZ); - imu_.mutable_angular_velocity()->set_x(pImuData->gyroX); - imu_.mutable_angular_velocity()->set_y(pImuData->gyroY); - imu_.mutable_angular_velocity()->set_z(pImuData->gyroZ); - - return true; +bool EnbroadParse::HandleIMUData(const enbroad::NAV_IMU_TypeDef* pImuData) { + float imu_measurement_span = 1.0f / 100.0f; + double seconds = + pImuData->gps_week * SECONDS_PER_WEEK + pImuData->gpssecond * 1e-3; + + imu_.set_measurement_time(seconds); + imu_.set_measurement_span(imu_measurement_span); + imu_.mutable_linear_acceleration()->set_x(pImuData->accX); + imu_.mutable_linear_acceleration()->set_y(pImuData->accY); + imu_.mutable_linear_acceleration()->set_z(pImuData->accZ); + imu_.mutable_angular_velocity()->set_x(pImuData->gyroX * DEG_TO_RAD); + imu_.mutable_angular_velocity()->set_y(pImuData->gyroY * DEG_TO_RAD); + imu_.mutable_angular_velocity()->set_z(pImuData->gyroZ * DEG_TO_RAD); + return true; } -bool EnbroadParse::HandleGNSSData(const enbroad::NAV_GNSS_TypeDef* pGnssData) -{ - //填充bestpos中有关gnss数据及heading所有数据 - double seconds = pGnssData->gps_week * SECONDS_PER_WEEK + pGnssData->gpssecond * 1e-3; - //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);//number of satellites tracked - bestpos_.set_num_sats_in_solution(pGnssData->satsNum);//number of satellites used in solution - bestpos_.set_num_sats_in_solution(pGnssData->satsNum);//number of L1/E1/B1 satellites used in solution - bestpos_.set_num_sats_multi(pGnssData->satsNum);//number of multi-frequency satellites used in solution - //bestpos_.set_galileo_beidou_used_mask(0); - //bestpos_.set_gps_glonass_used_mask(0); - /*********************************heading_****************************************************/ - heading_.set_measurement_time(seconds); - //gnss航向 - heading_.set_heading(pGnssData->heading); - heading_.set_baseline_length(pGnssData->baseline); - heading_.set_reserved(0); - //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);//number of satellites tracked - heading_.set_satellite_soulution_number(pGnssData->satsNum);//number of satellites used in solution - heading_.set_satellite_number_obs(pGnssData->satsNum);//number of L1/E1/B1 satellites used in solution - heading_.set_satellite_number_multi(pGnssData->satsNum);//number of multi-frequency satellites used in solution - //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); - } - return true; +bool EnbroadParse::HandleGNSSData(const enbroad::NAV_GNSS_TypeDef* pGnssData) { + double seconds = + pGnssData->gps_week * SECONDS_PER_WEEK + pGnssData->gpssecond * 1e-3; + // 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); + // bestpos_.set_galileo_beidou_used_mask(0); + // bestpos_.set_gps_glonass_used_mask(0); + + heading_.set_measurement_time(seconds); + heading_.set_heading(pGnssData->heading); + heading_.set_baseline_length(pGnssData->baseline); + heading_.set_reserved(0); + // 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); + } + return true; } -bool EnbroadParse::HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData) -{ - //static float senor_temp; - static unsigned short rtkStatus; - //static unsigned short headingStatus; - //static unsigned short velStatus; - //static unsigned short CanInfoFlag; - static unsigned short Nav_Standard_flag; - //static unsigned short Nav_Status; - static unsigned short Sate_Num; - static float baseline; - float imu_measurement_span = 1.0f / 100.0f; - - double seconds = pNavData->gps_week * SECONDS_PER_WEEK + pNavData->gps_millisecs * 1e-3; - /*********************************ins_****************************************************/ - ins_.set_measurement_time(seconds); - ins_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); - ins_.mutable_euler_angles()->set_x(double(pNavData->roll*Coder_Angle_Scale/Coder_Sensor_Scale* DEG_TO_RAD)); - ins_.mutable_euler_angles()->set_y(double(pNavData->pitch*Coder_Angle_Scale/Coder_Sensor_Scale* DEG_TO_RAD)); - //enbroad 北偏西为正，此处北偏东为正 - ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(normalizeAngleTo180(-pNavData->head*Coder_Angle_Scale/Coder_Sensor_Scale))); - ins_.mutable_position()->set_lon(double(pNavData->lon/Coder_Pos_Scale)); - ins_.mutable_position()->set_lat(double(pNavData->lat/Coder_Pos_Scale)); - ins_.mutable_position()->set_height(double(pNavData->alt/1000.0)); - ins_.mutable_linear_acceleration()->set_x(double(pNavData->accX*Coder_Accel_Scale/Coder_Sensor_Scale)); - ins_.mutable_linear_acceleration()->set_y(double(pNavData->accY*Coder_Accel_Scale/Coder_Sensor_Scale)); - ins_.mutable_linear_acceleration()->set_z(double(pNavData->accZ*Coder_Accel_Scale/Coder_Sensor_Scale)); - ins_.mutable_angular_velocity()->set_x(double(pNavData->gyroX*Coder_Rate_Scale/Coder_Sensor_Scale)); - ins_.mutable_angular_velocity()->set_y(double(pNavData->gyroY*Coder_Rate_Scale/Coder_Sensor_Scale)); - ins_.mutable_angular_velocity()->set_z(double(pNavData->gyroZ*Coder_Rate_Scale/Coder_Sensor_Scale)); - ins_.mutable_linear_velocity()->set_x(double(pNavData->ve*Coder_Vel_Scale/Coder_Sensor_Scale)); - ins_.mutable_linear_velocity()->set_y(double(pNavData->vn*Coder_Vel_Scale/Coder_Sensor_Scale)); - ins_.mutable_linear_velocity()->set_z(double(pNavData->vu*Coder_Vel_Scale/Coder_Sensor_Scale)); - ins_.set_type(Ins::GOOD); - - switch(pNavData->poll_type) - { - case enbroad::E_POLL_DEV_TEMP: - //senor_temp = pNavData->poll_frame1/Coder_Sensor_Scale*Coder_Temp_Scale; - break; - case enbroad::E_POLL_GNSS_STATE: - rtkStatus = pNavData->poll_frame1; - //headingStatus = pNavData->poll_frame2; - //velStatus = pNavData->poll_frame3; - break; - case enbroad::E_POLL_CAN_STATE: - //CanInfoFlag = pNavData->poll_frame1; - break; - case enbroad::E_POLL_INS_STATE: - Nav_Standard_flag= pNavData->poll_frame1; - //Nav_Status = pNavData->poll_frame2; - break; - case enbroad::E_POLL_GNSS2_STATE: - Sate_Num = pNavData->poll_frame1; - baseline = pNavData->poll_frame2/1000.0; - break; - } - - /*********************************imu_****************************************************/ - imu_.set_measurement_time(seconds); - imu_.set_measurement_span(imu_measurement_span); - imu_.mutable_linear_acceleration()->set_x(double(pNavData->accX*Coder_Accel_Scale/Coder_Sensor_Scale)); - imu_.mutable_linear_acceleration()->set_y(double(pNavData->accY*Coder_Accel_Scale/Coder_Sensor_Scale)); - imu_.mutable_linear_acceleration()->set_z(double(pNavData->accZ*Coder_Accel_Scale/Coder_Sensor_Scale)); - imu_.mutable_angular_velocity()->set_x(double(pNavData->gyroX*Coder_Rate_Scale/Coder_Sensor_Scale)); - imu_.mutable_angular_velocity()->set_y(double(pNavData->gyroY*Coder_Rate_Scale/Coder_Sensor_Scale)); - imu_.mutable_angular_velocity()->set_z(double(pNavData->gyroZ*Coder_Rate_Scale/Coder_Sensor_Scale)); - - /*********************************bestpos_****************************************************/ - bestpos_.set_measurement_time(seconds); - bestpos_.set_longitude(double(pNavData->lon/Coder_Pos_Scale)); - bestpos_.set_latitude(double(pNavData->lat/Coder_Pos_Scale)); - bestpos_.set_height_msl(double(pNavData->alt/1000.0)); - - - bestpos_.set_undulation(0.0);//undulation = height_wgs84 - height_msl - bestpos_.set_datum_id(static_cast(enbroad::DatumId::WGS84));//datum id number.WGS84 - bestpos_.set_latitude_std_dev(0.0); - bestpos_.set_longitude_std_dev(0.0); - bestpos_.set_height_std_dev(0.0); - bestpos_.set_base_station_id("0");//base station id - bestpos_.set_solution_age(0.0);//solution age (sec) - bestpos_.set_num_sats_tracked(Sate_Num);//number of satellites tracked - bestpos_.set_num_sats_in_solution(Sate_Num);//number of satellites used in solution - bestpos_.set_num_sats_in_solution(Sate_Num);//number of L1/E1/B1 satellites used in solution - bestpos_.set_num_sats_multi(Sate_Num);//number of multi-frequency satellites used in solution - bestpos_.set_extended_solution_status(SolutionType::INS_RTKFIXED);//extended solution status - OEMV and - bestpos_.set_galileo_beidou_used_mask(0); - bestpos_.set_gps_glonass_used_mask(0); - - /*********************************heading_****************************************************/ - heading_.set_measurement_time(seconds); - heading_.set_pitch(double(pNavData->pitch*Coder_Angle_Scale/Coder_Sensor_Scale)); - //enbroad 北偏西为正，此处北偏东为正 - heading_.set_heading(normalizeAngleTo180(-pNavData->head*Coder_Angle_Scale/Coder_Sensor_Scale)); - //AERROR << "heading_.set_heading: " << normalizeAngleTo180(-pNavData->head*Coder_Angle_Scale/Coder_Sensor_Scale); - heading_.set_baseline_length(baseline); - heading_.set_reserved(0); - heading_.set_heading_std_dev(0.0); - heading_.set_pitch_std_dev(0.0); - heading_.set_station_id("0"); - heading_.set_satellite_tracked_number(Sate_Num);//number of satellites tracked - heading_.set_satellite_soulution_number(Sate_Num);//number of satellites used in solution - heading_.set_satellite_number_obs(Sate_Num);//number of L1/E1/B1 satellites used in solution - heading_.set_satellite_number_multi(Sate_Num);//number of multi-frequency satellites used in solution - 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); - - /*********************************ins_stat_****************************************************/ - ins_stat_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); - //根据RTK状态， 固定为INS_RTKFIXED 浮点为INS_RTKFLOAT，单点及伪距差分SINGLE 无解：NONE - if(enbroad::E_GPS_RTK_FIXED == rtkStatus) - { - ins_stat_.set_pos_type(SolutionType::INS_RTKFIXED); - bestpos_.set_sol_type(SolutionType::INS_RTKFIXED); - heading_.set_position_type(SolutionType::INS_RTKFIXED); - } - else if(enbroad::E_GPS_RTK_FLOAT == rtkStatus) - { - ins_stat_.set_pos_type(SolutionType::INS_RTKFLOAT); - bestpos_.set_sol_type(SolutionType::INS_RTKFLOAT); - heading_.set_position_type(SolutionType::INS_RTKFLOAT); - } - else if(enbroad::E_GPS_RTK_SPP == rtkStatus || enbroad::E_GPS_RTK_DGPS == rtkStatus) - { - ins_stat_.set_pos_type(SolutionType::SINGLE); - bestpos_.set_sol_type(SolutionType::SINGLE); - heading_.set_position_type(SolutionType::SINGLE); - } - else - { - ins_stat_.set_pos_type(SolutionType::NONE); - bestpos_.set_sol_type(SolutionType::NONE); - heading_.set_position_type(SolutionType::NONE); - } - - //根据组合导航标定状态：1. 未标定SOL_COMPUTED 2. 标定中SOL_COMPUTED 3. 完成标定SOL_COMPUTED - if(enbroad::E_NAV_STANDARD_PROCCSSED == Nav_Standard_flag) - { - ins_stat_.set_ins_status(SolutionStatus::SOL_COMPUTED); - bestpos_.set_sol_status(SolutionStatus::SOL_COMPUTED); - heading_.set_solution_status(SolutionStatus::SOL_COMPUTED); - } - else if(enbroad::E_NAV_STANDARD_PROCCSSING == Nav_Standard_flag) - { - ins_stat_.set_ins_status(SolutionStatus::COLD_START); - bestpos_.set_sol_status(SolutionStatus::COLD_START); - heading_.set_solution_status(SolutionStatus::COLD_START); - } - else - { - ins_stat_.set_ins_status(SolutionStatus::INSUFFICIENT_OBS); - bestpos_.set_sol_status(SolutionStatus::INSUFFICIENT_OBS); - heading_.set_solution_status(SolutionStatus::INSUFFICIENT_OBS); - } - - - 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; + float imu_measurement_span = 1.0f / 100.0f; + double seconds = + pNavData->gps_week * SECONDS_PER_WEEK + pNavData->gps_millisecs * 1e-3; + ins_.set_measurement_time(seconds); + ins_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); + ins_.mutable_euler_angles()->set_x(static_cast( + pNavData->roll * Coder_Angle_Scale / Coder_Sensor_Scale * DEG_TO_RAD)); + ins_.mutable_euler_angles()->set_y(static_cast( + pNavData->pitch * Coder_Angle_Scale / Coder_Sensor_Scale * DEG_TO_RAD)); + // enbroad set northwest as right direction, Here northeast as right direction + ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(normalizeAngleTo180( + -pNavData->head * Coder_Angle_Scale / Coder_Sensor_Scale))); + ins_.mutable_position()->set_lon( + static_cast(pNavData->lon / Coder_Pos_Scale)); + ins_.mutable_position()->set_lat( + static_cast(pNavData->lat / Coder_Pos_Scale)); + ins_.mutable_position()->set_height( + static_cast(pNavData->alt / 1000.0)); + ins_.mutable_linear_acceleration()->set_x(static_cast( + pNavData->accX * Coder_Accel_Scale / Coder_Sensor_Scale)); + ins_.mutable_linear_acceleration()->set_y(static_cast( + pNavData->accY * Coder_Accel_Scale / Coder_Sensor_Scale)); + ins_.mutable_linear_acceleration()->set_z(static_cast( + pNavData->accZ * Coder_Accel_Scale / Coder_Sensor_Scale)); + ins_.mutable_angular_velocity()->set_x( + static_cast(pNavData->gyroX * Coder_Rate_Scale / + Coder_Sensor_Scale) * + DEG_TO_RAD); + ins_.mutable_angular_velocity()->set_y( + static_cast(pNavData->gyroY * Coder_Rate_Scale / + Coder_Sensor_Scale) * + DEG_TO_RAD); + ins_.mutable_angular_velocity()->set_z( + static_cast(pNavData->gyroZ * Coder_Rate_Scale / + Coder_Sensor_Scale) * + DEG_TO_RAD); + ins_.mutable_linear_velocity()->set_x( + static_cast(pNavData->ve * Coder_Vel_Scale / Coder_Sensor_Scale)); + ins_.mutable_linear_velocity()->set_y( + static_cast(pNavData->vn * Coder_Vel_Scale / Coder_Sensor_Scale)); + ins_.mutable_linear_velocity()->set_z( + static_cast(pNavData->vu * Coder_Vel_Scale / Coder_Sensor_Scale)); + ins_.set_type(Ins::GOOD); + switch (pNavData->poll_type) { + case enbroad::E_POLL_DEV_TEMP: + break; + case enbroad::E_POLL_GNSS_STATE: + rtkStatus = pNavData->poll_frame1; + break; + case enbroad::E_POLL_CAN_STATE: + break; + case enbroad::E_POLL_INS_STATE: + Nav_Standard_flag = pNavData->poll_frame1; + break; + case enbroad::E_POLL_GNSS2_STATE: + Sate_Num = pNavData->poll_frame1; + baseline = pNavData->poll_frame2 / 1000.0; + break; + default: + break; + } + imu_.set_measurement_time(seconds); + imu_.set_measurement_span(imu_measurement_span); + imu_.mutable_linear_acceleration()->set_x(static_cast( + pNavData->accX * Coder_Accel_Scale / Coder_Sensor_Scale)); + imu_.mutable_linear_acceleration()->set_y(static_cast( + pNavData->accY * Coder_Accel_Scale / Coder_Sensor_Scale)); + imu_.mutable_linear_acceleration()->set_z(static_cast( + pNavData->accZ * Coder_Accel_Scale / Coder_Sensor_Scale)); + imu_.mutable_angular_velocity()->set_x( + static_cast(pNavData->gyroX * Coder_Rate_Scale / + Coder_Sensor_Scale) * + DEG_TO_RAD); + imu_.mutable_angular_velocity()->set_y( + static_cast(pNavData->gyroY * Coder_Rate_Scale / + Coder_Sensor_Scale) * + DEG_TO_RAD); + imu_.mutable_angular_velocity()->set_z( + static_cast(pNavData->gyroZ * Coder_Rate_Scale / + Coder_Sensor_Scale) * + DEG_TO_RAD); + bestpos_.set_measurement_time(seconds); + bestpos_.set_longitude(static_cast(pNavData->lon / Coder_Pos_Scale)); + bestpos_.set_latitude(static_cast(pNavData->lat / Coder_Pos_Scale)); + bestpos_.set_height_msl(static_cast(pNavData->alt / 1000.0)); + bestpos_.set_undulation(0.0); // undulation = height_wgs84 - height_msl + bestpos_.set_datum_id( + static_cast(enbroad::DatumId::WGS84)); + bestpos_.set_latitude_std_dev(0.0); + bestpos_.set_longitude_std_dev(0.0); + bestpos_.set_height_std_dev(0.0); + bestpos_.set_base_station_id("0"); + bestpos_.set_solution_age(0.0); // solution age (sec) + bestpos_.set_num_sats_tracked(Sate_Num); + bestpos_.set_num_sats_in_solution(Sate_Num); + bestpos_.set_num_sats_in_solution(Sate_Num); + bestpos_.set_num_sats_multi(Sate_Num); + bestpos_.set_extended_solution_status(SolutionType::INS_RTKFIXED); + bestpos_.set_galileo_beidou_used_mask(0); + bestpos_.set_gps_glonass_used_mask(0); + heading_.set_measurement_time(seconds); + heading_.set_pitch(static_cast(pNavData->pitch * Coder_Angle_Scale / + Coder_Sensor_Scale)); + // enbroad set northwest as right direction, + // Here northeast as right direction + heading_.set_heading(normalizeAngleTo180(-pNavData->head * Coder_Angle_Scale / + Coder_Sensor_Scale)); + heading_.set_baseline_length(baseline); + heading_.set_reserved(0); + heading_.set_heading_std_dev(0.0); + heading_.set_pitch_std_dev(0.0); + heading_.set_station_id("0"); + heading_.set_satellite_tracked_number(Sate_Num); + heading_.set_satellite_soulution_number(Sate_Num); + heading_.set_satellite_number_obs(Sate_Num); + heading_.set_satellite_number_multi(Sate_Num); + 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); + ins_stat_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); + // According to the RTK state: + // fixed as "INS-RTKFIXED",float as "INS-RTKFLOAT", + // single or RTD as "SINGLE",no solution as "NONE" + if (enbroad::E_GPS_RTK_FIXED == rtkStatus) { + ins_stat_.set_pos_type(SolutionType::INS_RTKFIXED); + bestpos_.set_sol_type(SolutionType::INS_RTKFIXED); + heading_.set_position_type(SolutionType::INS_RTKFIXED); + } else if (enbroad::E_GPS_RTK_FLOAT == rtkStatus) { + ins_stat_.set_pos_type(SolutionType::INS_RTKFLOAT); + bestpos_.set_sol_type(SolutionType::INS_RTKFLOAT); + heading_.set_position_type(SolutionType::INS_RTKFLOAT); + } else if (enbroad::E_GPS_RTK_SPP == rtkStatus || + enbroad::E_GPS_RTK_DGPS == rtkStatus) { + ins_stat_.set_pos_type(SolutionType::SINGLE); + bestpos_.set_sol_type(SolutionType::SINGLE); + heading_.set_position_type(SolutionType::SINGLE); + } else { + ins_stat_.set_pos_type(SolutionType::NONE); + bestpos_.set_sol_type(SolutionType::NONE); + heading_.set_position_type(SolutionType::NONE); + } + // According to sins calibration status: + // no calibration as "SOL_COMPUTED", + // during calibration as "SOL_COMPUTED", + // completing calibration as "SOL_COMPUTED" + if (enbroad::E_NAV_STANDARD_PROCCSSED == Nav_Standard_flag) { + ins_stat_.set_ins_status(SolutionStatus::SOL_COMPUTED); + bestpos_.set_sol_status(SolutionStatus::SOL_COMPUTED); + heading_.set_solution_status(SolutionStatus::SOL_COMPUTED); + } else if (enbroad::E_NAV_STANDARD_PROCCSSING == Nav_Standard_flag) { + ins_stat_.set_ins_status(SolutionStatus::COLD_START); + bestpos_.set_sol_status(SolutionStatus::COLD_START); + heading_.set_solution_status(SolutionStatus::COLD_START); + } else { + ins_stat_.set_ins_status(SolutionStatus::INSUFFICIENT_OBS); + bestpos_.set_sol_status(SolutionStatus::INSUFFICIENT_OBS); + heading_.set_solution_status(SolutionStatus::INSUFFICIENT_OBS); + } + return true; } } // namespace gnss } // namespace drivers } // namespace apollo - -#endif - From 6e01ee011b2002d2a4490ee107c439ba9244d679 Mon Sep 17 00:00:00 2001 From: dengwei19840721 Date: Fri, 24 May 2024 08:57:42 +0800 Subject: [PATCH 4/5] Restore default configuration gnss_conf.pb.txt --- modules/drivers/gnss/conf/gnss_conf.pb.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/drivers/gnss/conf/gnss_conf.pb.txt b/modules/drivers/gnss/conf/gnss_conf.pb.txt index 40c0c7c22f6..b1e712af8b0 100644 --- a/modules/drivers/gnss/conf/gnss_conf.pb.txt +++ b/modules/drivers/gnss/conf/gnss_conf.pb.txt @@ -1,10 +1,10 @@ data { - # format optional: NOVATEL_BINARY, HUACE_TEXT, ASENSING_BINARY, BROADGNSS_TEXT,ENBROAD_BINARY + # format optional: NOVATEL_BINARY, HUACE_TEXT, ASENSING_BINARY, BROADGNSS_TEXT format: NOVATEL_BINARY # protocol optional: serial, tcp, udp, ntrip, can_card_parameter serial { - device: "/dev/novatel0" + device: "/dev/ttyACM0" baud_rate: 115200 } @@ -39,7 +39,7 @@ data { rtk_to { format: NOVATEL_BINARY serial { - device: "/dev/novatel1" + device: "/dev/ttyACM1" baud_rate: 115200 } } @@ -47,7 +47,7 @@ rtk_to { command { format: NOVATEL_BINARY serial { - device: "/dev/novatel2" + device: "/dev/ttyACM2" baud_rate: 115200 } } From 9b8ee147c48b87b8133a93fcecfc43742eea17eb Mon Sep 17 00:00:00 2001 From: dengwei19840721 Date: Tue, 8 Oct 2024 18:29:50 +0800 Subject: [PATCH 5/5] feat:add new protocol --- .../parser/enbroad_parser/enbroad_messages.h | 28 ++- .../parser/enbroad_parser/enbroad_parser.cc | 231 ++++++++++++------ 2 files changed, 184 insertions(+), 75 deletions(-) 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;