Apollo 10.0
自动驾驶开放平台
steering_65.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 Steering65::ID = 0x65;
28
29void Steering65::Parse(const std::uint8_t *bytes, int32_t length,
30 Lincoln *chassis_detail) const {
31 chassis_detail->mutable_eps()->set_steering_angle(
32 steering_angle(bytes, length));
33 // no steering angle speed
34
35 chassis_detail->mutable_eps()->set_steering_angle_cmd(
36 reported_steering_angle_cmd(bytes, length));
37 // ?
38 chassis_detail->mutable_eps()->set_is_steering_angle_valid(true);
39 // vehicle speed from steering, kph -> mps
40 chassis_detail->mutable_eps()->set_vehicle_speed(
41 vehicle_speed(bytes, length) / 3.6);
42
43 // speed, as it has a higher accuracy
44 // kph -> mps
45 chassis_detail->mutable_vehicle_spd()->set_vehicle_spd(
46 vehicle_speed(bytes, length) / 3.6);
47 chassis_detail->mutable_vehicle_spd()->set_is_vehicle_spd_valid(true);
48
49 chassis_detail->mutable_eps()->set_epas_torque(epas_torque(bytes, length));
50 chassis_detail->mutable_eps()->set_steering_enabled(
51 is_enabled(bytes, length));
52 chassis_detail->mutable_eps()->set_driver_override(
53 is_driver_override(bytes, length));
54 chassis_detail->mutable_eps()->set_driver_activity(
55 is_driver_activity(bytes, length));
56 chassis_detail->mutable_eps()->set_watchdog_fault(
57 is_watchdog_counter_fault(bytes, length));
58 chassis_detail->mutable_eps()->set_channel_1_fault(
59 is_channel_1_fault(bytes, length));
60 chassis_detail->mutable_eps()->set_channel_2_fault(
61 is_channel_2_fault(bytes, length));
62 chassis_detail->mutable_eps()->set_calibration_fault(
63 is_calibration_fault(bytes, length));
64 chassis_detail->mutable_eps()->set_connector_fault(
65 is_connector_fault(bytes, length));
66 chassis_detail->mutable_check_response()->set_is_eps_online(
67 !is_driver_override(bytes, length));
68}
69
70void Steering65::Parse(const std::uint8_t *bytes, int32_t length,
71 const struct timeval &timestamp,
72 Lincoln *chassis_detail) const {
73 chassis_detail->mutable_eps()->set_timestamp_65(
74 static_cast<double>(timestamp.tv_sec) +
75 static_cast<double>(timestamp.tv_usec) / 1000000.0);
76 Parse(bytes, length, chassis_detail);
77}
78
79double Steering65::steering_angle(const std::uint8_t *bytes,
80 int32_t length) const {
81 Byte frame_high(bytes + 1);
82 int32_t high = frame_high.get_byte(0, 8);
83 Byte frame_low(bytes + 0);
84 int32_t low = frame_low.get_byte(0, 8);
85 int32_t value = (high << 8) | low;
86 if (value > 0x8000) {
87 value = value - 0x10000;
88 }
89
90 return value * 0.100000;
91}
92
93double Steering65::reported_steering_angle_cmd(const std::uint8_t *bytes,
94 int32_t length) const {
95 Byte frame_high(bytes + 3);
96 int32_t high = frame_high.get_byte(0, 8);
97 Byte frame_low(bytes + 2);
98 int32_t low = frame_low.get_byte(0, 8);
99 int32_t value = (high << 8) | low;
100 if (value > 0x8000) {
101 value = value - 0x10000;
102 }
103
104 return value * 0.100000;
105}
106
107double Steering65::vehicle_speed(const std::uint8_t *bytes,
108 int32_t length) const {
109 Byte frame_high(bytes + 5);
110 int32_t high = frame_high.get_byte(0, 8);
111 Byte frame_low(bytes + 4);
112 int32_t low = frame_low.get_byte(0, 8);
113 int32_t value = (high << 8) | low;
114 return value * 0.010000;
115}
116
117double Steering65::epas_torque(const std::uint8_t *bytes,
118 int32_t length) const {
119 Byte frame(bytes + 6);
120 int32_t x = frame.get_byte(0, 8);
121 if (x > 0x7F) {
122 x -= 0x100;
123 }
124 return x * 0.062500;
125}
126
127bool Steering65::is_enabled(const std::uint8_t *bytes, int32_t length) const {
128 Byte frame(bytes + 7);
129 return frame.is_bit_1(0);
130}
131
132bool Steering65::is_driver_override(const std::uint8_t *bytes,
133 int32_t length) const {
134 // Cleared on rising edge of EN bit in command message
135 Byte frame(bytes + 7);
136 return frame.is_bit_1(1);
137}
138
139bool Steering65::is_driver_activity(const std::uint8_t *bytes,
140 int32_t length) const {
141 Byte frame(bytes + 7);
142 return frame.is_bit_1(2);
143}
144
145bool Steering65::is_watchdog_counter_fault(const std::uint8_t *bytes,
146 int32_t length) const {
147 Byte frame(bytes + 7);
148 return frame.is_bit_1(3);
149}
150
151bool Steering65::is_channel_1_fault(const std::uint8_t *bytes,
152 int32_t length) const {
153 Byte frame(bytes + 7);
154 return frame.is_bit_1(4);
155}
156
157bool Steering65::is_channel_2_fault(const std::uint8_t *bytes,
158 int32_t length) const {
159 Byte frame(bytes + 7);
160 return frame.is_bit_1(5);
161}
162
163bool Steering65::is_calibration_fault(const std::uint8_t *bytes,
164 int32_t length) const {
165 Byte frame(bytes + 7);
166 return frame.is_bit_1(6);
167}
168
169bool Steering65::is_connector_fault(const std::uint8_t *bytes,
170 int32_t length) const {
171 Byte frame(bytes + 7);
172 return frame.is_bit_1(7);
173}
174
175} // namespace lincoln
176} // namespace canbus
177} // namespace apollo
Defines the Byte class.
double epas_torque(const std::uint8_t *bytes, int32_t length) const
calculate epas torque based on byte array.
bool is_enabled(const std::uint8_t *bytes, int32_t length) const
check enabled bit based on byte array.
double reported_steering_angle_cmd(const std::uint8_t *bytes, int32_t length) const
calculate reported steering angle command based on byte array.
bool is_connector_fault(const std::uint8_t *bytes, int32_t length) const
check connector fault bit based on byte array.
bool is_channel_2_fault(const std::uint8_t *bytes, int32_t length) const
check channel 2 fault bit based on byte array.
virtual void Parse(const std::uint8_t *bytes, int32_t length, Lincoln *chassis_detail) const
bool is_watchdog_counter_fault(const std::uint8_t *bytes, int32_t length) const
check watchdog counter fault based on byte array.
bool is_driver_activity(const std::uint8_t *bytes, int32_t length) const
check driver activity bit based on byte array.
bool is_driver_override(const std::uint8_t *bytes, int32_t length) const
check driver override bit based on byte array.
bool is_channel_1_fault(const std::uint8_t *bytes, int32_t length) const
check channel 1 fault bit based on byte array.
bool is_calibration_fault(const std::uint8_t *bytes, int32_t length) const
check calibration fault bit based on byte array.
double steering_angle(const std::uint8_t *bytes, int32_t length) const
calculate steering angle based on byte array.
double vehicle_speed(const std::uint8_t *bytes, int32_t length) const
calculate vehicle speed based on byte array.
class register implement
Definition arena_queue.h:37
the class of steering_65.h (for lincoln vehicle)