Apollo 10.0
自动驾驶开放平台
surround_73.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 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
18
20
21namespace apollo {
22namespace canbus {
23namespace lincoln {
24
25using ::apollo::drivers::canbus::Byte;
26
27const int32_t Surround73::ID = 0x73;
28
29void Surround73::Parse(const std::uint8_t *bytes, int32_t length,
30 Lincoln *chassis_detail) const {
31 // sonar left
32 chassis_detail->mutable_surround()->set_cross_traffic_alert_left(
33 is_cross_traffic_alert_left(bytes, length));
34 chassis_detail->mutable_surround()->set_cross_traffic_alert_left_enabled(
35 is_cross_traffic_alert_left_enabled(bytes, length));
36
37 // blind spot left
38 chassis_detail->mutable_surround()->set_blind_spot_left_alert(
39 is_blind_spot_left_alert(bytes, length));
40 chassis_detail->mutable_surround()->set_blind_spot_left_alert_enabled(
41 is_blind_spot_left_alert_enabled(bytes, length));
42
43 // sonar right
44 chassis_detail->mutable_surround()->set_cross_traffic_alert_right(
45 is_cross_traffic_alert_right(bytes, length));
46 chassis_detail->mutable_surround()->set_cross_traffic_alert_left_enabled(
47 is_cross_traffic_alert_left_enabled(bytes, length));
48
49 // blind spot right
50 chassis_detail->mutable_surround()->set_blind_spot_right_alert(
51 is_blind_spot_right_alert(bytes, length));
52 chassis_detail->mutable_surround()->set_blind_spot_right_alert_enabled(
53 is_blind_spot_right_alert_enabled(bytes, length));
54
55 // sonar00 ~ sonar11, output in meters
56 chassis_detail->mutable_surround()->set_sonar00(sonar00(bytes, length));
57 chassis_detail->mutable_surround()->set_sonar01(sonar01(bytes, length));
58 chassis_detail->mutable_surround()->set_sonar02(sonar02(bytes, length));
59 chassis_detail->mutable_surround()->set_sonar03(sonar03(bytes, length));
60 chassis_detail->mutable_surround()->set_sonar04(sonar04(bytes, length));
61 chassis_detail->mutable_surround()->set_sonar05(sonar05(bytes, length));
62 chassis_detail->mutable_surround()->set_sonar06(sonar06(bytes, length));
63 chassis_detail->mutable_surround()->set_sonar07(sonar07(bytes, length));
64 chassis_detail->mutable_surround()->set_sonar08(sonar08(bytes, length));
65 chassis_detail->mutable_surround()->set_sonar09(sonar09(bytes, length));
66 chassis_detail->mutable_surround()->set_sonar10(sonar10(bytes, length));
67 chassis_detail->mutable_surround()->set_sonar11(sonar11(bytes, length));
68
69 // alternative representations.
70 const int8_t kSonarNumbers = 12;
71 chassis_detail->mutable_surround()->clear_sonar_range();
72 for (std::int8_t i = 0; i < kSonarNumbers; ++i) {
73 chassis_detail->mutable_surround()->add_sonar_range(
74 sonars(bytes, i, length / 2));
75 }
76}
77
78bool Surround73::is_cross_traffic_alert_left(const std::uint8_t *bytes,
79 int32_t length) const {
80 Byte frame(bytes + 0);
81 return frame.is_bit_1(0);
82}
83
84bool Surround73::is_cross_traffic_alert_left_enabled(const std::uint8_t *bytes,
85 int32_t length) const {
86 Byte frame(bytes + 0);
87 return frame.is_bit_1(1);
88}
89
90bool Surround73::is_blind_spot_left_alert(const std::uint8_t *bytes,
91 int32_t length) const {
92 Byte frame(bytes + 0);
93 return frame.is_bit_1(2);
94}
95
96bool Surround73::is_blind_spot_left_alert_enabled(const std::uint8_t *bytes,
97 int32_t length) const {
98 Byte frame(bytes + 0);
99 return frame.is_bit_1(3);
100}
101
102bool Surround73::is_cross_traffic_alert_right(const std::uint8_t *bytes,
103 int32_t length) const {
104 Byte frame(bytes + 0);
105 return frame.is_bit_1(4);
106}
107
108bool Surround73::is_cross_traffic_alert_right_enabled(const std::uint8_t *bytes,
109 int32_t length) const {
110 Byte frame(bytes + 0);
111 return frame.is_bit_1(5);
112}
113
114bool Surround73::is_blind_spot_right_alert(const std::uint8_t *bytes,
115 int32_t length) const {
116 Byte frame(bytes + 0);
117 return frame.is_bit_1(6);
118}
119
120bool Surround73::is_blind_spot_right_alert_enabled(const std::uint8_t *bytes,
121 int32_t length) const {
122 Byte frame(bytes + 0);
123 return frame.is_bit_1(7);
124}
125
126double Surround73::sonar00(const std::uint8_t *bytes, int32_t length) const {
127 Byte frame(bytes + 1);
128 int32_t x = frame.get_byte(0, 3);
129 return sonar_range(x);
130}
131
132double Surround73::sonar01(const std::uint8_t *bytes, int32_t length) const {
133 Byte frame(bytes + 1);
134 int32_t x = frame.get_byte(4, 7);
135 return sonar_range(x);
136}
137
138double Surround73::sonar02(const std::uint8_t *bytes, int32_t length) const {
139 Byte frame(bytes + 2);
140 int32_t x = frame.get_byte(0, 3);
141 return sonar_range(x);
142}
143
144double Surround73::sonar03(const std::uint8_t *bytes, int32_t length) const {
145 Byte frame(bytes + 2);
146 int32_t x = frame.get_byte(4, 7);
147 return sonar_range(x);
148}
149
150double Surround73::sonar04(const std::uint8_t *bytes, int32_t length) const {
151 Byte frame(bytes + 3);
152 int32_t x = frame.get_byte(0, 3);
153 return sonar_range(x);
154}
155
156double Surround73::sonar05(const std::uint8_t *bytes, int32_t length) const {
157 Byte frame(bytes + 3);
158 int32_t x = frame.get_byte(4, 7);
159 return sonar_range(x);
160}
161
162double Surround73::sonar06(const std::uint8_t *bytes, int32_t length) const {
163 Byte frame(bytes + 4);
164 int32_t x = frame.get_byte(0, 3);
165 return sonar_range(x);
166}
167
168double Surround73::sonar07(const std::uint8_t *bytes, int32_t length) const {
169 Byte frame(bytes + 4);
170 int32_t x = frame.get_byte(4, 7);
171 return sonar_range(x);
172}
173
174double Surround73::sonar08(const std::uint8_t *bytes, int32_t length) const {
175 Byte frame(bytes + 5);
176 int32_t x = frame.get_byte(0, 3);
177 return sonar_range(x);
178}
179
180double Surround73::sonar09(const std::uint8_t *bytes, int32_t length) const {
181 Byte frame(bytes + 5);
182 int32_t x = frame.get_byte(4, 7);
183 return sonar_range(x);
184}
185
186double Surround73::sonar10(const std::uint8_t *bytes, int32_t length) const {
187 Byte frame(bytes + 6);
188 int32_t x = frame.get_byte(0, 3);
189 return sonar_range(x);
190}
191
192double Surround73::sonar11(const std::uint8_t *bytes, int32_t length) const {
193 Byte frame(bytes + 6);
194 int32_t x = frame.get_byte(4, 7);
195 return sonar_range(x);
196}
197
198bool Surround73::sonar_enabled(const std::uint8_t *bytes,
199 int32_t length) const {
200 Byte frame(bytes + 7);
201 return frame.is_bit_1(6);
202}
203
204bool Surround73::sonar_fault(const std::uint8_t *bytes, int32_t length) const {
205 Byte frame(bytes + 7);
206 return frame.is_bit_1(7);
207}
208
209double Surround73::sonar_range(const std::int32_t x) const {
210 if (x == 0x0) {
211 return 100.0; // If nothing detected, set test range to 100.0m.
212 }
213 return 0.145 * (x - 0x1) + 0.3;
214}
215
216double Surround73::sonars(const std::uint8_t *bytes, std::uint8_t sonar_number,
217 int32_t length) const {
218 Byte frame(bytes + sonar_number / 2 + 1);
219 int32_t start = (sonar_number % 2) * length;
220 int32_t x = frame.get_byte(start, start + length);
221 return sonar_range(x);
222}
223
224} // namespace lincoln
225} // namespace canbus
226} // namespace apollo
Defines the Byte class.
virtual void Parse(const std::uint8_t *bytes, int32_t length, Lincoln *chassis_detail) const
class register implement
Definition arena_queue.h:37
the class of Surround73 (for lincoln vehicle)