Skip to content

Commit 2ac412c

Browse files
oldfu12138oldfu12138
authored
feat: add forsense gnss device (#15492)
Add forsense gnss device in apollo drivers
1 parent fb23f6a commit 2ac412c

File tree

5 files changed

+440
-0
lines changed

5 files changed

+440
-0
lines changed

‎modules/drivers/gnss/parser/forsense_parser/forsense_base_parser.cc‎

Lines changed: 184 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,184 @@
1+
/******************************************************************************
2+
* Copyright 2024 The Apollo Authors. All Rights Reserved.
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*****************************************************************************/
16+
17+
// An parser for decoding binary messages from a NovAtel receiver. The following
18+
// messages must be
19+
// logged in order for this parser to work properly.
20+
//
21+
#include "modules/drivers/gnss/parser/forsense_parser/forsense_base_parser.h"
22+
23+
#include <cmath>
24+
#include <iostream>
25+
#include <limits>
26+
#include <memory>
27+
#include <vector>
28+
29+
namespace apollo {
30+
namespace drivers {
31+
namespace gnss {
32+
33+
ForsenseBaseParser::ForsenseBaseParser(const config::Config& config) {}
34+
35+
void ForsenseBaseParser::GetMessages(MessageInfoVec* messages) {
36+
if (data_ == nullptr) {
37+
return;
38+
}
39+
if (!PrepareMessage()) {
40+
return;
41+
}
42+
43+
FillGnssBestpos();
44+
FillImu();
45+
FillHeading();
46+
FillIns();
47+
FillInsStat();
48+
49+
messages->push_back(MessageInfo{MessageType::BEST_GNSS_POS,
50+
reinterpret_cast<MessagePtr>(&bestpos_)});
51+
messages->push_back(
52+
MessageInfo{MessageType::IMU, reinterpret_cast<MessagePtr>(&imu_)});
53+
messages->push_back(MessageInfo{MessageType::HEADING,
54+
reinterpret_cast<MessagePtr>(&heading_)});
55+
messages->push_back(
56+
MessageInfo{MessageType::INS, reinterpret_cast<MessagePtr>(&ins_)});
57+
messages->push_back(MessageInfo{MessageType::INS_STAT,
58+
reinterpret_cast<MessagePtr>(&ins_stat_)});
59+
}
60+
61+
void ForsenseBaseParser::PrepareMessageStatus(const uint8_t& system_state,
62+
const uint8_t& satellite_status) {
63+
switch (system_state) {
64+
case 0:
65+
decode_message_.solution_status = SolutionStatus::COLD_START;
66+
break;
67+
case 1:
68+
case 2:
69+
decode_message_.solution_status = SolutionStatus::SOL_COMPUTED;
70+
break;
71+
default:
72+
decode_message_.solution_status = SolutionStatus::INSUFFICIENT_OBS;
73+
}
74+
switch (satellite_status) {
75+
case 0:
76+
decode_message_.solution_type = SolutionType::NONE;
77+
break;
78+
case 1:
79+
case 6:
80+
decode_message_.solution_type = SolutionType::SINGLE;
81+
break;
82+
case 2:
83+
case 7:
84+
decode_message_.solution_type = SolutionType::PSRDIFF;
85+
break;
86+
case 3:
87+
decode_message_.solution_type = SolutionType::PROPOGATED;
88+
break;
89+
case 4:
90+
decode_message_.solution_type = SolutionType::INS_RTKFIXED;
91+
break;
92+
case 5:
93+
decode_message_.solution_type = SolutionType::INS_RTKFLOAT;
94+
break;
95+
case 8:
96+
decode_message_.solution_type = SolutionType::NARROW_INT;
97+
break;
98+
case 9:
99+
decode_message_.solution_type = SolutionType::NARROW_FLOAT;
100+
break;
101+
default:
102+
decode_message_.solution_type = SolutionType::NONE;
103+
}
104+
}
105+
106+
void ForsenseBaseParser::FillGnssBestpos() {
107+
bestpos_.set_measurement_time(decode_message_.gps_timestamp_sec);
108+
bestpos_.set_sol_status(decode_message_.solution_status);
109+
bestpos_.set_sol_type(decode_message_.solution_type);
110+
bestpos_.set_latitude(decode_message_.Latitude);
111+
bestpos_.set_longitude(decode_message_.Longitude);
112+
bestpos_.set_height_msl(decode_message_.Altitude);
113+
bestpos_.set_latitude_std_dev(decode_message_.lat_std);
114+
bestpos_.set_longitude_std_dev(decode_message_.lon_std);
115+
bestpos_.set_height_std_dev(decode_message_.alti_std);
116+
bestpos_.set_num_sats_tracked(decode_message_.satellites_num);
117+
bestpos_.set_num_sats_in_solution(decode_message_.satellites_num);
118+
bestpos_.set_num_sats_l1(decode_message_.satellites_num);
119+
bestpos_.set_num_sats_multi(decode_message_.satellites_num);
120+
}
121+
122+
void ForsenseBaseParser::FillIns() {
123+
ins_.mutable_euler_angles()->set_x(decode_message_.Roll * DEG_TO_RAD);
124+
ins_.mutable_euler_angles()->set_y(-decode_message_.Pitch * DEG_TO_RAD);
125+
ins_.mutable_euler_angles()->set_z(
126+
azimuth_deg_to_yaw_rad(decode_message_.Heading));
127+
ins_.mutable_position()->set_lon(decode_message_.Longitude);
128+
ins_.mutable_position()->set_lat(decode_message_.Latitude);
129+
ins_.mutable_position()->set_height(decode_message_.Altitude);
130+
ins_.mutable_linear_velocity()->set_x(decode_message_.Ve);
131+
ins_.mutable_linear_velocity()->set_y(decode_message_.Vn);
132+
ins_.mutable_linear_velocity()->set_z(decode_message_.Vu);
133+
rfu_to_flu(decode_message_.AccX, decode_message_.AccY, decode_message_.AccZ,
134+
ins_.mutable_linear_acceleration());
135+
rfu_to_flu(decode_message_.GyroX, decode_message_.GyroY,
136+
decode_message_.GyroZ, ins_.mutable_angular_velocity());
137+
ins_.set_measurement_time(decode_message_.gps_timestamp_sec);
138+
ins_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond());
139+
140+
switch (decode_message_.solution_type) {
141+
case SolutionType::INS_RTKFIXED:
142+
case SolutionType::NARROW_INT:
143+
case SolutionType::INS_RTKFLOAT:
144+
case SolutionType::NARROW_FLOAT:
145+
ins_.set_type(Ins::GOOD);
146+
break;
147+
case SolutionType::SINGLE:
148+
ins_.set_type(Ins::CONVERGING);
149+
break;
150+
default:
151+
ins_.set_type(Ins::INVALID);
152+
break;
153+
}
154+
}
155+
156+
void ForsenseBaseParser::FillInsStat() {
157+
ins_stat_.set_ins_status(decode_message_.solution_status);
158+
ins_stat_.set_pos_type(decode_message_.solution_type);
159+
}
160+
161+
void ForsenseBaseParser::FillImu() {
162+
rfu_to_flu(decode_message_.AccX, decode_message_.AccY, decode_message_.AccZ,
163+
imu_.mutable_linear_acceleration());
164+
rfu_to_flu(decode_message_.GyroX, decode_message_.GyroY,
165+
decode_message_.GyroZ, imu_.mutable_angular_velocity());
166+
imu_.set_measurement_time(decode_message_.gps_timestamp_sec);
167+
}
168+
169+
void ForsenseBaseParser::FillHeading() {
170+
heading_.set_solution_status(decode_message_.solution_status);
171+
heading_.set_position_type(decode_message_.solution_type);
172+
heading_.set_measurement_time(decode_message_.gps_timestamp_sec);
173+
heading_.set_heading(decode_message_.Heading);
174+
heading_.set_pitch(decode_message_.Pitch);
175+
heading_.set_heading_std_dev(decode_message_.yaw_std);
176+
heading_.set_pitch_std_dev(decode_message_.pitch_std);
177+
// heading_.set_station_id("0");
178+
heading_.set_satellite_number_multi(decode_message_.satellites_num);
179+
heading_.set_satellite_soulution_number(decode_message_.satellites_num);
180+
}
181+
182+
} // namespace gnss
183+
} // namespace drivers
184+
} // namespace apollo

‎modules/drivers/gnss/parser/forsense_parser/forsense_base_parser.h‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
/******************************************************************************
2+
* Copyright 2024 The Apollo Authors. All Rights Reserved.
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*****************************************************************************/
16+
17+
// An parser for decoding binary messages from a NovAtel receiver. The following
18+
// messages must be
19+
// logged in order for this parser to work properly.
20+
//
21+
#include <string>
22+
23+
#include "modules/common_msgs/sensor_msgs/gnss.pb.h"
24+
#include "modules/common_msgs/sensor_msgs/gnss_best_pose.pb.h"
25+
#include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h"
26+
#include "modules/common_msgs/sensor_msgs/heading.pb.h"
27+
#include "modules/common_msgs/sensor_msgs/imu.pb.h"
28+
#include "modules/common_msgs/sensor_msgs/ins.pb.h"
29+
30+
#include "cyber/cyber.h"
31+
#include "modules/drivers/gnss/parser/parser.h"
32+
#include "modules/drivers/gnss/parser/parser_common.h"
33+
#include "modules/drivers/gnss/util/time_conversion.h"
34+
35+
namespace apollo {
36+
namespace drivers {
37+
namespace gnss {
38+
39+
struct ForsenseMessage {
40+
std::string messageID;
41+
int GPSWeek;
42+
double GPSTime;
43+
double Heading;
44+
double Pitch;
45+
double Roll;
46+
double GyroX;
47+
double GyroY;
48+
double GyroZ;
49+
double AccX;
50+
double AccY;
51+
double AccZ;
52+
double Latitude;
53+
double Longitude;
54+
double Altitude;
55+
double Ve;
56+
double Vn;
57+
double Vu;
58+
double V;
59+
int NSV1;
60+
int NSV2;
61+
SolutionStatus solution_status;
62+
SolutionType solution_type;
63+
double Age;
64+
std::string WarningCs;
65+
uint8_t satellites_num = 0;
66+
uint8_t wheel_speed_status = 0;
67+
double gps_timestamp_sec = 0;
68+
// 数据标准差
69+
double lat_std = 0.0;
70+
double lon_std = 0.0;
71+
double alti_std = 0.0;
72+
double vn_std = 0.0;
73+
double ve_std = 0.0;
74+
double vu_std = 0.0;
75+
double v_std = 0.0;
76+
77+
double roll_std = 0.0;
78+
double pitch_std = 0.0;
79+
double yaw_std = 0.0;
80+
};
81+
82+
class ForsenseBaseParser : public Parser {
83+
public:
84+
ForsenseBaseParser() {}
85+
explicit ForsenseBaseParser(const config::Config &config);
86+
87+
virtual bool PrepareMessage() = 0;
88+
89+
virtual void GetMessages(MessageInfoVec *messages);
90+
91+
protected:
92+
void PrepareMessageStatus(const uint8_t &system_state,
93+
const uint8_t &satellite_status);
94+
ForsenseMessage decode_message_;
95+
96+
private:
97+
void FillGnssBestpos();
98+
void FillIns();
99+
void FillInsStat();
100+
void FillImu();
101+
void FillHeading();
102+
103+
GnssBestPose bestpos_;
104+
Imu imu_;
105+
Heading heading_;
106+
Ins ins_;
107+
InsStat ins_stat_;
108+
};
109+
110+
} // namespace gnss
111+
} // namespace drivers
112+
} // namespace apollo

0 commit comments

Comments
 (0)