25#define ACC_UNIT 0.0001220703 - 4
26#define G_UNIT 9.7883105
27#define GYRO_UNIT 0.0076293 - 250
28#define ANGLE_UNIT 0.010986 - 360
29#define ALT_UINT 0.001 - 10000
30#define LOC_UNIT 1E-07 - 180
31#define SPEED_UNIT 0.0030517 - 100
32#define LOC_STD_UNIT 0.001
33#define HEADING_STD_UNIT 0.01
99T
ToValue(
const uint8_t *data,
const int &index) {
101 for (uint8_t i = 0; i <
sizeof(T); ++i) {
102 t |=
static_cast<T
>(data[index + i]) << ((
sizeof(T) - i - 1) * 8);
107template <
typename T0,
typename T1>
108T1
ToValue(
const uint8_t *data,
const int &index) {
109 return static_cast<T1
>(ToValue<T0>(data, index));
130 void DecodeInsLatitudeLongitude(
136 void DecodeGpsLatitudeLongitude(
143 uint32_t cur_can_id_;
151 if (
data_ ==
nullptr) {
198 cur_can_id_ = can_frame->id;
208 DecodeInsGyro(frame);
211 DecodeInsAngle(frame);
214 DecodeInsHeightAndTime(frame);
217 DecodeInsLatitudeLongitude(frame);
220 DecodeInsSpeed(frame);
223 DecodeInsDataInfo(frame);
229 DecodeGnssUtc(frame);
232 DecodeGpsLatitudeLongitude(frame);
235 DecodeGpsHeightAndWeek(frame);
238 DecodeGpsHeadingAndPitch(frame);
244 AWARN <<
"Unknown CAN frame id: " << std::hex << frame.
id;
249void AsensingCanParser::DecodeInsAcc(
256void AsensingCanParser::DecodeInsGyro(
265void AsensingCanParser::DecodeInsAngle(
274void AsensingCanParser::DecodeInsHeightAndTime(
279 static_cast<double>(adata_.
week_ms) / 1000;
281void AsensingCanParser::DecodeInsLatitudeLongitude(
286void AsensingCanParser::DecodeInsSpeed(
292void AsensingCanParser::DecodeInsDataInfo(
294 uint8_t flag_pos = frame.
data[0];
295 if (SolutionType_IsValid(flag_pos)) {
301 switch (frame.
data[7]) {
309void AsensingCanParser::DecodeInsStd(
316void AsensingCanParser::DecodeGnssUtc(
318 struct tm utc_tm = {.tm_sec = frame.
data[5],
319 .tm_min = frame.
data[4],
320 .tm_hour = frame.
data[3],
321 .tm_mday = frame.
data[2],
322 .tm_mon = frame.
data[1] - 1,
323 .tm_year = frame.
data[0] + 2000 - 1900};
325 double ms = ToValue<uint16_t, double>(frame.
data, 6) * 0.001;
326 if (timestamp != -1) {
330 AERROR <<
"Invalid utc time: " << utc_tm.tm_year <<
"-" << utc_tm.tm_mon
331 <<
"-" << utc_tm.tm_mday <<
" " << utc_tm.tm_hour <<
":"
332 << utc_tm.tm_min <<
":" << utc_tm.tm_sec <<
"." << ms;
336void AsensingCanParser::DecodeGpsLatitudeLongitude(
341void AsensingCanParser::DecodeGpsHeightAndWeek(
346void AsensingCanParser::DecodeGpsHeadingAndPitch(
354void AsensingCanParser::DecodeGpsStd(
397 ins_.mutable_linear_velocity()->set_x(adata_.
ve);
398 ins_.mutable_linear_velocity()->set_y(adata_.
vn);
399 ins_.mutable_linear_velocity()->set_z(-adata_.
vd);
401 ins_.mutable_linear_acceleration());
403 ins_.mutable_angular_velocity());
430 imu_.mutable_linear_acceleration());
432 imu_.mutable_angular_velocity());
Defines the CanFrame struct and CanClient interface.
static Time Now()
get the current time.
SolutionType solution_type_
RateControl bestpos_ratecontrol_
SolutionStatus solution_status_
AsensingCanParser(const config::Config &config)
void GetMessages(MessageInfoVec *messages) override
void FillGnssBestpos() override
void FillInsStat() override
bool PrepareMessage() override
void FillHeading() override
static Parser * CreateAsensingCan(const config::Config &config)
constexpr uint32_t FRAME_ins_gyro
::google::protobuf::Message * MessagePtr
constexpr uint32_t FRAME_ins_datainfo
constexpr uint32_t FRAME_gps_std
constexpr uint32_t FRAME_ins_latitude_longitude
constexpr double DEG_TO_RAD
void rfu_to_flu(double r, double f, double u, ::apollo::common::Point3D *flu)
constexpr uint32_t FRAME_ins_height_and_time
T ToValue(const uint8_t *data, const int &index)
constexpr int SECONDS_PER_WEEK
constexpr uint32_t FRAME_ins_std
constexpr uint32_t FRAME_gps_heading_and_pitch
constexpr uint32_t FRAME_ins_speed
constexpr uint32_t FRAME_gps_height_and_week
constexpr double azimuth_deg_to_yaw_rad(double azimuth)
constexpr uint32_t FRAME_gps_latitude_longitude
std::vector< MessageInfo > MessageInfoVec
constexpr uint32_t FRAME_ins_acc
constexpr uint32_t FRAME_ins_angle
constexpr uint32_t FRAME_gnss_utc
T unix2gps(const T unix_seconds)
The class which defines the information to send and receive.
std::string CanFrameString() const
CanFrame string including essential information about the message.
uint8_t data[8]
Message content
SolutionStatus solution_status
SolutionType solution_type