Apollo 10.0
自动驾驶开放平台
wheelspeed_6a.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
19#include "glog/logging.h"
20
22
23namespace apollo {
24namespace canbus {
25namespace lincoln {
26
27using ::apollo::drivers::canbus::Byte;
28
29const int32_t Wheelspeed6a::ID = 0x6A;
30
31void Wheelspeed6a::Parse(const std::uint8_t *bytes, int32_t length,
32 Lincoln *chassis_detail) const {
33 // how to set direction
34 // what is "valid"
35 // front left
36 chassis_detail->mutable_vehicle_spd()->set_wheel_spd_fl(
37 front_left_wheel_speed(bytes, length));
38 chassis_detail->mutable_vehicle_spd()->set_is_wheel_spd_fl_valid(true);
39 chassis_detail->mutable_vehicle_spd()->set_wheel_direction_fl(
40 wheel_direction_convert(front_left_wheel_speed(bytes, length)));
41 // front right
42 chassis_detail->mutable_vehicle_spd()->set_wheel_spd_fr(
43 front_right_wheel_speed(bytes, length));
44 chassis_detail->mutable_vehicle_spd()->set_is_wheel_spd_fr_valid(true);
45 chassis_detail->mutable_vehicle_spd()->set_wheel_direction_fr(
46 wheel_direction_convert(front_right_wheel_speed(bytes, length)));
47 // rear left
48 chassis_detail->mutable_vehicle_spd()->set_wheel_spd_rl(
49 rear_left_wheel_speed(bytes, length));
50 chassis_detail->mutable_vehicle_spd()->set_is_wheel_spd_rl_valid(true);
51 chassis_detail->mutable_vehicle_spd()->set_wheel_direction_rl(
52 wheel_direction_convert(rear_left_wheel_speed(bytes, length)));
53 // rear right
54 chassis_detail->mutable_vehicle_spd()->set_wheel_spd_rr(
55 rear_right_wheel_speed(bytes, length));
56 chassis_detail->mutable_vehicle_spd()->set_is_wheel_spd_rr_valid(true);
57 chassis_detail->mutable_vehicle_spd()->set_wheel_direction_rr(
58 wheel_direction_convert(rear_right_wheel_speed(bytes, length)));
59 /*
60 -?(rr(bytes, length));
61 -?(rl(bytes, length));
62 -?(fr(bytes, length));
63 -?(fl(bytes, length));*/
64}
65
66void Wheelspeed6a::Parse(const std::uint8_t *bytes, int32_t length,
67 const struct timeval &timestamp,
68 Lincoln *chassis_detail) const {
69 chassis_detail->mutable_vehicle_spd()->set_timestamp_sec(
70 static_cast<double>(timestamp.tv_sec) +
71 static_cast<double>(timestamp.tv_usec) / 1000000.0);
72 Parse(bytes, length, chassis_detail);
73}
74
75double Wheelspeed6a::front_left_wheel_speed(const std::uint8_t *bytes,
76 int32_t length) const {
77 DCHECK_GE(length, 2);
78 return parse_two_frames(bytes[0], bytes[1]);
79}
80
81double Wheelspeed6a::front_right_wheel_speed(const std::uint8_t *bytes,
82 int32_t length) const {
83 DCHECK_GE(length, 4);
84 return parse_two_frames(bytes[2], bytes[3]);
85}
86
87double Wheelspeed6a::rear_left_wheel_speed(const std::uint8_t *bytes,
88 int32_t length) const {
89 DCHECK_GE(length, 6);
90 return parse_two_frames(bytes[4], bytes[5]);
91}
92
93double Wheelspeed6a::rear_right_wheel_speed(const std::uint8_t *bytes,
94 int32_t length) const {
95 DCHECK_GE(length, 8);
96 return parse_two_frames(bytes[6], bytes[7]);
97}
98
99double Wheelspeed6a::parse_two_frames(const std::uint8_t low_byte,
100 const std::uint8_t high_byte) const {
101 Byte high_frame(&high_byte);
102 int32_t high = high_frame.get_byte(0, 8);
103 Byte low_frame(&low_byte);
104 int32_t low = low_frame.get_byte(0, 8);
105 int32_t value = (high << 8) | low;
106 if (value > 0x7FFF) {
107 value -= 0x10000;
108 }
109 return value * 0.010000;
110}
111
112WheelSpeed::WheelSpeedType Wheelspeed6a::wheel_direction_convert(
113 double wheel_speed) const {
115 if (wheel_speed > 0) {
116 wheel_direction = WheelSpeed::FORWARD;
117 } else if ((wheel_speed > -0.01) && (wheel_speed < 0.01)) {
118 wheel_direction = WheelSpeed::STANDSTILL;
119 } else if (wheel_speed < 0) {
120 wheel_direction = WheelSpeed::BACKWARD;
121 } else {
122 wheel_direction = WheelSpeed::INVALID;
123 }
124 return wheel_direction;
125}
126
127} // namespace lincoln
128} // namespace canbus
129} // 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