pacmod3_core.cpp
Go to the documentation of this file.
1 /*
2 * Unpublished Copyright (c) 2009-2017 AutonomouStuff, LLC, All Rights Reserved.
3 *
4 * This file is part of the PACMod v3 ROS 1.0 driver which is released under the MIT license.
5 * See file LICENSE included with this software or go to https://opensource.org/licenses/MIT for full license details.
6 */
7 
8 #include <pacmod3_core.h>
9 
10 using namespace AS::Drivers::PACMod3;
11 
12 const int64_t AS::Drivers::PACMod3::GlobalRptMsg::CAN_ID = 0x10;
13 
14 // System Commands
15 const int64_t AS::Drivers::PACMod3::AccelCmdMsg::CAN_ID = 0x100;
16 const int64_t AS::Drivers::PACMod3::BrakeCmdMsg::CAN_ID = 0x104;
17 const int64_t AS::Drivers::PACMod3::CruiseControlButtonsCmdMsg::CAN_ID = 0x108;
18 const int64_t AS::Drivers::PACMod3::DashControlsLeftCmdMsg::CAN_ID = 0x10C;
19 const int64_t AS::Drivers::PACMod3::DashControlsRightCmdMsg::CAN_ID = 0x110;
20 const int64_t AS::Drivers::PACMod3::HazardLightCmdMsg::CAN_ID = 0x114;
21 const int64_t AS::Drivers::PACMod3::HeadlightCmdMsg::CAN_ID = 0x118;
22 const int64_t AS::Drivers::PACMod3::HornCmdMsg::CAN_ID = 0x11C;
23 const int64_t AS::Drivers::PACMod3::MediaControlsCmdMsg::CAN_ID = 0x120;
24 const int64_t AS::Drivers::PACMod3::ParkingBrakeCmdMsg::CAN_ID = 0x124;
25 const int64_t AS::Drivers::PACMod3::ShiftCmdMsg::CAN_ID = 0x128;
26 const int64_t AS::Drivers::PACMod3::SteerCmdMsg::CAN_ID = 0x12C;
27 const int64_t AS::Drivers::PACMod3::TurnSignalCmdMsg::CAN_ID = 0x130;
28 const int64_t AS::Drivers::PACMod3::WiperCmdMsg::CAN_ID = 0x134;
29 
30 // System Reports
31 const int64_t AS::Drivers::PACMod3::AccelRptMsg::CAN_ID = 0x200;
32 const int64_t AS::Drivers::PACMod3::BrakeRptMsg::CAN_ID = 0x204;
33 const int64_t AS::Drivers::PACMod3::CruiseControlButtonsRptMsg::CAN_ID = 0x208;
34 const int64_t AS::Drivers::PACMod3::DashControlsLeftRptMsg::CAN_ID = 0x20C;
35 const int64_t AS::Drivers::PACMod3::DashControlsRightRptMsg::CAN_ID = 0x210;
36 const int64_t AS::Drivers::PACMod3::HazardLightRptMsg::CAN_ID = 0x214;
37 const int64_t AS::Drivers::PACMod3::HeadlightRptMsg::CAN_ID = 0x218;
38 const int64_t AS::Drivers::PACMod3::HornRptMsg::CAN_ID = 0x21C;
39 const int64_t AS::Drivers::PACMod3::MediaControlsRptMsg::CAN_ID = 0x220;
40 const int64_t AS::Drivers::PACMod3::ParkingBrakeRptMsg::CAN_ID = 0x224;
41 const int64_t AS::Drivers::PACMod3::ShiftRptMsg::CAN_ID = 0x228;
42 const int64_t AS::Drivers::PACMod3::SteerRptMsg::CAN_ID = 0x22C;
43 const int64_t AS::Drivers::PACMod3::TurnSignalRptMsg::CAN_ID = 0x230;
44 const int64_t AS::Drivers::PACMod3::WiperRptMsg::CAN_ID = 0x234;
45 
46 // System Aux Reports
47 const int64_t AS::Drivers::PACMod3::AccelAuxRptMsg::CAN_ID = 0x300;
48 const int64_t AS::Drivers::PACMod3::BrakeAuxRptMsg::CAN_ID = 0x304;
49 const int64_t AS::Drivers::PACMod3::HeadlightAuxRptMsg::CAN_ID = 0x318;
50 const int64_t AS::Drivers::PACMod3::ShiftAuxRptMsg::CAN_ID = 0x328;
51 const int64_t AS::Drivers::PACMod3::SteerAuxRptMsg::CAN_ID = 0x32C;
52 const int64_t AS::Drivers::PACMod3::TurnAuxRptMsg::CAN_ID = 0x330;
53 const int64_t AS::Drivers::PACMod3::WiperAuxRptMsg::CAN_ID = 0x334;
54 
55 // Misc. Reports
56 const int64_t AS::Drivers::PACMod3::VehicleSpeedRptMsg::CAN_ID = 0x400;
57 const int64_t AS::Drivers::PACMod3::BrakeMotorRpt1Msg::CAN_ID = 0x401;
58 const int64_t AS::Drivers::PACMod3::BrakeMotorRpt2Msg::CAN_ID = 0x402;
59 const int64_t AS::Drivers::PACMod3::BrakeMotorRpt3Msg::CAN_ID = 0x403;
60 const int64_t AS::Drivers::PACMod3::SteerMotorRpt1Msg::CAN_ID = 0x404;
61 const int64_t AS::Drivers::PACMod3::SteerMotorRpt2Msg::CAN_ID = 0x405;
62 const int64_t AS::Drivers::PACMod3::SteerMotorRpt3Msg::CAN_ID = 0x406;
63 const int64_t AS::Drivers::PACMod3::WheelSpeedRptMsg::CAN_ID = 0x407;
64 const int64_t AS::Drivers::PACMod3::SteeringPIDRpt1Msg::CAN_ID = 0x408;
65 const int64_t AS::Drivers::PACMod3::SteeringPIDRpt2Msg::CAN_ID = 0x409;
66 const int64_t AS::Drivers::PACMod3::SteeringPIDRpt3Msg::CAN_ID = 0x40A;
67 const int64_t AS::Drivers::PACMod3::YawRateRptMsg::CAN_ID = 0x40D;
68 const int64_t AS::Drivers::PACMod3::LatLonHeadingRptMsg::CAN_ID = 0x40E;
69 const int64_t AS::Drivers::PACMod3::DateTimeRptMsg::CAN_ID = 0x40F;
70 const int64_t AS::Drivers::PACMod3::SteeringPIDRpt4Msg::CAN_ID = 0x410;
71 const int64_t AS::Drivers::PACMod3::DetectedObjectRptMsg::CAN_ID = 0x411;
72 const int64_t AS::Drivers::PACMod3::VehicleSpecificRpt1Msg::CAN_ID = 0x412;
73 const int64_t AS::Drivers::PACMod3::VehicleDynamicsRptMsg::CAN_ID = 0x413;
74 const int64_t AS::Drivers::PACMod3::VinRptMsg::CAN_ID = 0x414;
75 const int64_t AS::Drivers::PACMod3::OccupancyRptMsg::CAN_ID = 0x415;
76 const int64_t AS::Drivers::PACMod3::InteriorLightsRptMsg::CAN_ID = 0x416;
77 const int64_t AS::Drivers::PACMod3::DoorRptMsg::CAN_ID = 0x417;
78 const int64_t AS::Drivers::PACMod3::RearLightsRptMsg::CAN_ID = 0x418;
79 
80 std::shared_ptr<Pacmod3TxMsg> Pacmod3TxMsg::make_message(const int64_t& can_id)
81 {
82  switch (can_id)
83  {
84  case AccelRptMsg::CAN_ID:
85  return std::shared_ptr<Pacmod3TxMsg>(new AccelRptMsg);
86  break;
87  case BrakeMotorRpt1Msg::CAN_ID:
88  return std::shared_ptr<Pacmod3TxMsg>(new BrakeMotorRpt1Msg);
89  break;
90  case BrakeMotorRpt2Msg::CAN_ID:
91  return std::shared_ptr<Pacmod3TxMsg>(new BrakeMotorRpt2Msg);
92  break;
93  case BrakeMotorRpt3Msg::CAN_ID:
94  return std::shared_ptr<Pacmod3TxMsg>(new BrakeMotorRpt3Msg);
95  break;
96  case BrakeRptMsg::CAN_ID:
97  return std::shared_ptr<Pacmod3TxMsg>(new BrakeRptMsg);
98  break;
99  case CruiseControlButtonsRptMsg::CAN_ID:
100  return std::shared_ptr<Pacmod3TxMsg>(new CruiseControlButtonsRptMsg);
101  break;
102  case DashControlsLeftRptMsg::CAN_ID:
103  return std::shared_ptr<Pacmod3TxMsg>(new DashControlsLeftRptMsg);
104  break;
105  case DashControlsRightRptMsg::CAN_ID:
106  return std::shared_ptr<Pacmod3TxMsg>(new DashControlsRightRptMsg);
107  break;
108  case DateTimeRptMsg::CAN_ID:
109  return std::shared_ptr<Pacmod3TxMsg>(new DateTimeRptMsg);
110  break;
111  case DetectedObjectRptMsg::CAN_ID:
112  return std::shared_ptr<Pacmod3TxMsg>(new DetectedObjectRptMsg);
113  break;
114  case DoorRptMsg::CAN_ID:
115  return std::shared_ptr<Pacmod3TxMsg>(new DoorRptMsg);
116  break;
117  case GlobalRptMsg::CAN_ID:
118  return std::shared_ptr<Pacmod3TxMsg>(new GlobalRptMsg);
119  break;
120  case HeadlightRptMsg::CAN_ID:
121  return std::shared_ptr<Pacmod3TxMsg>(new HeadlightRptMsg);
122  break;
123  case HornRptMsg::CAN_ID:
124  return std::shared_ptr<Pacmod3TxMsg>(new HornRptMsg);
125  break;
126  case InteriorLightsRptMsg::CAN_ID:
127  return std::shared_ptr<Pacmod3TxMsg>(new InteriorLightsRptMsg);
128  break;
129  case LatLonHeadingRptMsg::CAN_ID:
130  return std::shared_ptr<Pacmod3TxMsg>(new LatLonHeadingRptMsg);
131  break;
132  case MediaControlsRptMsg::CAN_ID:
133  return std::shared_ptr<Pacmod3TxMsg>(new MediaControlsRptMsg);
134  break;
135  case OccupancyRptMsg::CAN_ID:
136  return std::shared_ptr<Pacmod3TxMsg>(new OccupancyRptMsg);
137  break;
138  case ParkingBrakeRptMsg::CAN_ID:
139  return std::shared_ptr<Pacmod3TxMsg>(new ParkingBrakeRptMsg);
140  break;
141  case RearLightsRptMsg::CAN_ID:
142  return std::shared_ptr<Pacmod3TxMsg>(new RearLightsRptMsg);
143  break;
144  case ShiftRptMsg::CAN_ID:
145  return std::shared_ptr<Pacmod3TxMsg>(new ShiftRptMsg);
146  break;
147  case SteeringPIDRpt1Msg::CAN_ID:
148  return std::shared_ptr<Pacmod3TxMsg>(new SteeringPIDRpt1Msg);
149  break;
150  case SteeringPIDRpt2Msg::CAN_ID:
151  return std::shared_ptr<Pacmod3TxMsg>(new SteeringPIDRpt2Msg);
152  break;
153  case SteeringPIDRpt3Msg::CAN_ID:
154  return std::shared_ptr<Pacmod3TxMsg>(new SteeringPIDRpt3Msg);
155  break;
156  case SteeringPIDRpt4Msg::CAN_ID:
157  return std::shared_ptr<Pacmod3TxMsg>(new SteeringPIDRpt4Msg);
158  break;
159  case SteerMotorRpt1Msg::CAN_ID:
160  return std::shared_ptr<Pacmod3TxMsg>(new SteerMotorRpt1Msg);
161  break;
162  case SteerMotorRpt2Msg::CAN_ID:
163  return std::shared_ptr<Pacmod3TxMsg>(new SteerMotorRpt2Msg);
164  break;
165  case SteerMotorRpt3Msg::CAN_ID:
166  return std::shared_ptr<Pacmod3TxMsg>(new SteerMotorRpt3Msg);
167  break;
168  case SteerRptMsg::CAN_ID:
169  return std::shared_ptr<Pacmod3TxMsg>(new SteerRptMsg);
170  break;
171  case TurnSignalRptMsg::CAN_ID:
172  return std::shared_ptr<Pacmod3TxMsg>(new TurnSignalRptMsg);
173  break;
174  case VehicleSpecificRpt1Msg::CAN_ID:
175  return std::shared_ptr<Pacmod3TxMsg>(new VehicleSpecificRpt1Msg);
176  break;
177  case VehicleDynamicsRptMsg::CAN_ID:
178  return std::shared_ptr<Pacmod3TxMsg>(new VehicleDynamicsRptMsg);
179  break;
180  case VehicleSpeedRptMsg::CAN_ID:
181  return std::shared_ptr<Pacmod3TxMsg>(new VehicleSpeedRptMsg);
182  break;
183  case VinRptMsg::CAN_ID:
184  return std::shared_ptr<Pacmod3TxMsg>(new VinRptMsg);
185  break;
186  case WheelSpeedRptMsg::CAN_ID:
187  return std::shared_ptr<Pacmod3TxMsg>(new WheelSpeedRptMsg);
188  break;
189  case WiperRptMsg::CAN_ID:
190  return std::shared_ptr<Pacmod3TxMsg>(new WiperRptMsg);
191  break;
192  case YawRateRptMsg::CAN_ID:
193  return std::shared_ptr<Pacmod3TxMsg>(new YawRateRptMsg);
194  break;
195  case AccelAuxRptMsg::CAN_ID:
196  return std::shared_ptr<Pacmod3TxMsg>(new AccelAuxRptMsg);
197  break;
198  case BrakeAuxRptMsg::CAN_ID:
199  return std::shared_ptr<Pacmod3TxMsg>(new BrakeAuxRptMsg);
200  break;
201  case HeadlightAuxRptMsg::CAN_ID:
202  return std::shared_ptr<Pacmod3TxMsg>(new HeadlightAuxRptMsg);
203  break;
204  case ShiftAuxRptMsg::CAN_ID:
205  return std::shared_ptr<Pacmod3TxMsg>(new ShiftAuxRptMsg);
206  break;
207  case SteerAuxRptMsg::CAN_ID:
208  return std::shared_ptr<Pacmod3TxMsg>(new SteerAuxRptMsg);
209  break;
210  case TurnAuxRptMsg::CAN_ID:
211  return std::shared_ptr<Pacmod3TxMsg>(new TurnAuxRptMsg);
212  break;
213  case WiperAuxRptMsg::CAN_ID:
214  return std::shared_ptr<Pacmod3TxMsg>(new WiperAuxRptMsg);
215  break;
216  default:
217  return NULL;
218  }
219 }
220 
221 bool Pacmod3TxMsg::isSystem()
222 {
223  return false;
224 }
225 
226 SystemRptMsg::SystemRptMsg() :
227  Pacmod3TxMsg(),
228  enabled(false),
229  override_active(false),
230  command_output_fault(false),
231  input_output_fault(false),
232  output_reported_fault(false),
233  pacmod_fault(false),
234  vehicle_fault(false)
235 {}
236 
237 bool SystemRptMsg::isSystem()
238 {
239  return true;
240 }
241 
242 SystemRptBoolMsg::SystemRptBoolMsg() :
243  SystemRptMsg(),
244  manual_input(false),
245  command(false),
246  output(false)
247 {}
248 
249 SystemRptIntMsg::SystemRptIntMsg() :
250  SystemRptMsg(),
251  manual_input(0),
252  command(0),
253  output(0)
254 {}
255 
256 SystemRptFloatMsg::SystemRptFloatMsg() :
257  SystemRptMsg(),
258  manual_input(0),
259  command(0),
260  output(0)
261 {}
262 
263 // TX Messages
264 void GlobalRptMsg::parse(uint8_t *in)
265 {
266  enabled = in[0] & 0x01;
267  override_active = ((in[0] & 0x02) > 0);
268  fault_active = ((in[0] & 0x80) > 0);
269  config_fault_active = ((in[1] & 0x01) > 0);
270  user_can_timeout = ((in[0] & 0x04) > 0);
271  steering_can_timeout = ((in[0] & 0x08) > 0);
272  brake_can_timeout = ((in[0] & 0x10) > 0);
273  subsystem_can_timeout = ((in[0] & 0x20) > 0);
274  vehicle_can_timeout = ((in[0] & 0x40) > 0);
275  user_can_read_errors = ((in[6] << 8) | in[7]);
276 }
277 
278 void SystemRptBoolMsg::parse(uint8_t *in)
279 {
280  enabled = ((in[0] & 0x01) > 0);
281  override_active = ((in[0] & 0x02) > 0);
282  command_output_fault = ((in[0] & 0x04) > 0);
283  input_output_fault = ((in[0] & 0x08) > 0);
284  output_reported_fault = ((in[0] & 0x10) > 0);
285  pacmod_fault = ((in[0] & 0x20) > 0);
286  vehicle_fault = ((in[0] & 0x40) > 0);
287 
288  manual_input = ((in[1] & 0x01) > 0);
289  command = ((in[2] & 0x01) > 0);
290  output = ((in[3] & 0x01) > 0);
291 }
292 
293 void SystemRptIntMsg::parse(uint8_t *in)
294 {
295  enabled = ((in[0] & 0x01) > 0);
296  override_active = ((in[0] & 0x02) > 0);
297  command_output_fault = ((in[0] & 0x04) > 0);
298  input_output_fault = ((in[0] & 0x08) > 0);
299  output_reported_fault = ((in[0] & 0x10) > 0);
300  pacmod_fault = ((in[0] & 0x20) > 0);
301  vehicle_fault = ((in[0] & 0x40) > 0);
302 
303  manual_input = in[1];
304  command = in[2];
305  output = in[3];
306 }
307 
308 void SystemRptFloatMsg::parse(uint8_t *in)
309 {
310  enabled = ((in[0] & 0x01) > 0);
311  override_active = ((in[0] & 0x02) > 0);
312  command_output_fault = ((in[0] & 0x04) > 0);
313  input_output_fault = ((in[0] & 0x08) > 0);
314  output_reported_fault = ((in[0] & 0x10) > 0);
315  pacmod_fault = ((in[0] & 0x20) > 0);
316  vehicle_fault = ((in[0] & 0x40) > 0);
317 
318  int16_t temp;
319 
320  temp = ((int16_t)in[1] << 8) | in[2];
321  manual_input = (double)(temp / 1000.0);
322 
323  temp = ((int16_t)in[3] << 8) | in[4];
324  command = (double)(temp / 1000.0);
325 
326  temp = ((int16_t)in[5] << 8) | in[6];
327  output = (double)(temp / 1000.0);
328 }
329 
330 void AccelAuxRptMsg::parse(uint8_t *in)
331 {
332  int16_t temp;
333 
334  temp = ((int16_t)in[0] << 8) | in[1];
335  raw_pedal_pos = (float)temp / 1000.0;
336 
337  temp = ((int16_t)in[2] << 8) | in[3];
338  raw_pedal_force = (float)temp / 1000.0;
339 
340  user_interaction = (in[4] & 0x01) > 0;
341  raw_pedal_pos_is_valid = (in[5] & 0x01) > 0;
342  raw_pedal_force_is_valid = (in[5] & 0x02) > 0;
343  user_interaction_is_valid = (in[5] & 0x04) > 0;
344 }
345 
346 void BrakeAuxRptMsg::parse(uint8_t *in)
347 {
348  int16_t temp;
349 
350  temp = ((int16_t)in[0] << 8) | in[1];
351  raw_pedal_pos = (float)temp / 1000.0;
352 
353  temp = ((int16_t)in[2] << 8) | in[3];
354  raw_pedal_force = (float)temp / 1000.0;
355 
356  temp = ((int16_t)in[4] << 8) | in[5];
357  raw_brake_pressure = (float)temp / 1000.0;
358 
359  user_interaction = (in[6] & 0x01) > 0;
360  brake_on_off = (in[6] & 0x02) > 0;
361  raw_pedal_pos_is_valid = (in[7] & 0x01) > 0;
362  raw_pedal_force_is_valid = (in[7] & 0x02) > 0;
363  raw_brake_pressure_is_valid = (in[7] & 0x04) > 0;
364  user_interaction_is_valid = (in[7] & 0x08) > 0;
365  brake_on_off_is_valid = (in[7] & 0x10) > 0;
366 }
367 
368 void DateTimeRptMsg::parse(uint8_t *in)
369 {
370  year = in[0];
371  month = in[1];
372  day = in[2];
373  hour = in[3];
374  minute = in[4];
375  second = in[5];
376 }
377 
378 void DetectedObjectRptMsg::parse(uint8_t *in)
379 {
380  int16_t temp;
381 
382  temp = (((int16_t)in[0] << 8) | in[1]);
383  front_object_distance_low_res = (double)(temp / 1000.0);
384 
385  temp = (((int16_t)in[2] << 8) | in[3]);
386  front_object_distance_high_res = (double)(temp / 1000.0);
387 }
388 
389 void DoorRptMsg::parse(uint8_t *in)
390 {
391  driver_door_open = ((in[0] & 0x01) > 0);
392  driver_door_open_is_valid = ((in[1] & 0x01) > 0);
393  passenger_door_open = ((in[0] & 0x02) > 0);
394  passenger_door_open_is_valid = ((in[1] & 0x02) > 0);
395  rear_driver_door_open = ((in[0] & 0x04) > 0);
396  rear_driver_door_open_is_valid = ((in[1] & 0x04) > 0);
397  rear_passenger_door_open = ((in[0] & 0x08) > 0);
398  rear_passenger_door_open_is_valid = ((in[1] & 0x08) > 0);
399  hood_open = ((in[0] & 0x10) > 0);
400  hood_open_is_valid = ((in[1] & 0x10) > 0);
401  trunk_open = ((in[0] & 0x20) > 0);
402  trunk_open_is_valid = ((in[1] & 0x20) > 0);
403  fuel_door_open = ((in[0] & 0x40) > 0);
404  fuel_door_open_is_valid = ((in[1] & 0x40) > 0);
405 }
406 
407 void HeadlightAuxRptMsg::parse(uint8_t *in)
408 {
409  headlights_on = (in[0] & 0x01) > 0;
410  headlights_on_bright = (in[0] & 0x02) > 0;
411  fog_lights_on = (in[0] & 0x04) > 0;
412  headlights_mode = in[1];
413  headlights_on_is_valid = (in[2] & 0x01) > 0;
414  headlights_on_bright_is_valid = (in[2] & 0x02) > 0;
415  fog_lights_on = (in[2] & 0x04) > 0;
416  headlights_mode_is_valid = (in[2] & 0x08) > 0;
417 }
418 
419 void InteriorLightsRptMsg::parse(uint8_t *in)
420 {
421  front_dome_lights_on = ((in[0] & 0x01) > 0);
422  front_dome_lights_on_is_valid = ((in[2] & 0x01) > 0);
423  rear_dome_lights_on = ((in[0] & 0x02) > 0);
424  rear_dome_lights_on_is_valid = ((in[2] & 0x02) > 0);
425  mood_lights_on = ((in[0] & 0x04) > 0);
426  mood_lights_on_is_valid = ((in[2] & 0x04) > 0);
427  dim_level = (DimLevel)in[1];
428  dim_level_is_valid = ((in[2] & 0x08) > 0);
429 }
430 
431 void LatLonHeadingRptMsg::parse(uint8_t *in)
432 {
433  latitude_degrees = (int8_t)in[0];
434  latitude_minutes = (uint8_t)in[1];
435  latitude_seconds = (uint8_t)in[2];
436  longitude_degrees = (int8_t)in[3];
437  longitude_minutes = (uint8_t)in[4];
438  longitude_seconds = (uint8_t)in[5];
439  heading = (((int16_t)in[6] << 8) | in[7]) / 100.0;
440 }
441 
442 void MotorRpt1Msg::parse(uint8_t *in)
443 {
444  int32_t temp;
445 
446  temp = ((int32_t)in[0] << 24) | ((int32_t)in[1] << 16) | ((int32_t)in[2] << 8) | in[3];
447  current = (double)(temp / 1000.0);
448 
449  temp = ((int32_t)in[4] << 24) | ((int32_t)in[5] << 16) | ((int32_t)in[6] << 8) | in[7];
450  position = (double)(temp / 1000.0);
451 }
452 
453 void MotorRpt2Msg::parse(uint8_t *in)
454 {
455  int16_t temp16;
456  int32_t temp32;
457 
458  temp16 = ((int16_t)in[0] << 8) | in[1];
459  encoder_temp = (double)temp16;
460 
461  temp16 = ((int16_t)in[2] << 8) | in[3];
462  motor_temp = (double)temp16;
463 
464  temp32 = ((int32_t)in[7] << 24) | ((int32_t)in[6] << 16) | ((int32_t)in[5] << 8) | in[4];
465  velocity = (double)(temp32 / 1000.0);
466 }
467 
468 void MotorRpt3Msg::parse(uint8_t *in)
469 {
470  int32_t temp;
471 
472  temp = ((int32_t)in[0] << 24) | ((int32_t)in[1] << 16) | ((int32_t)in[2] << 8) | in[3];
473  torque_output = (double)(temp / 1000.0);
474 
475  temp = ((int32_t)in[4] << 24) | ((int32_t)in[5] << 16) | ((int32_t)in[6] << 8) | in[7];
476  torque_input = (double)(temp / 1000.0);
477 }
478 
479 void OccupancyRptMsg::parse(uint8_t *in)
480 {
481  driver_seat_occupied = ((in[0] & 0x01) > 0);
482  driver_seat_occupied_is_valid = ((in[1] & 0x01) > 0);
483  passenger_seat_occupied = ((in[0] & 0x02) > 0);
484  passenger_seat_occupied_is_valid = ((in[1] & 0x02) > 0);
485  rear_seat_occupied = ((in[0] & 0x04) > 0);
486  rear_seat_occupied_is_valid = ((in[1] & 0x04) > 0);
487  driver_seatbelt_buckled = ((in[0] & 0x08) > 0);
488  driver_seatbelt_buckled_is_valid = ((in[1] & 0x08) > 0);
489  passenger_seatbelt_buckled = ((in[0] & 0x10) > 0);
490  passenger_seatbelt_buckled_is_valid = ((in[1] & 0x10) > 0);
491  rear_seatbelt_buckled = ((in[0] & 0x20) > 0);
492  rear_seatbelt_buckled_is_valid = ((in[1] & 0x20) > 0);
493 }
494 
495 void RearLightsRptMsg::parse(uint8_t *in)
496 {
497  brake_lights_on = ((in[0] & 0x01) > 0);
498  brake_lights_on_is_valid = ((in[1] & 0x01) > 0);
499  reverse_lights_on = ((in[0] & 0x02) > 0);
500  reverse_lights_on_is_valid = ((in[1] & 0x02) > 0);
501 }
502 
503 void ShiftAuxRptMsg::parse(uint8_t *in)
504 {
505  between_gears = (in[0] & 0x01) > 0;
506  stay_in_neutral_mode = (in[0] & 0x02) > 0;
507  brake_interlock_active = (in[0] & 0x04) > 0;
508  speed_interlock_active = (in[0] & 0x08) > 0;
509  between_gears_is_valid = (in[1] & 0x01) > 0;
510  stay_in_neutral_mode_is_valid = (in[1] & 0x02) > 0;
511  brake_interlock_active_is_valid = (in[1] & 0x04) > 0;
512  speed_interlock_active_is_valid = (in[1] & 0x08) > 0;
513 }
514 
515 void SteerAuxRptMsg::parse(uint8_t *in)
516 {
517  int16_t temp;
518 
519  temp = ((int16_t)in[0] << 8) | in[1];
520  raw_position = (float)temp / 10.0;
521 
522  temp = ((int16_t)in[2] << 8) | in[3];
523  raw_torque = (float)temp / 10.0;
524 
525  uint16_t temp2;
526 
527  temp2 = ((uint16_t)in[4] << 8) | in[5];
528  rotation_rate = (float)temp2 / 100.0;
529 
530  user_interaction = (in[6] & 0x01) > 0;
531  raw_position_is_valid = (in[7] & 0x01) > 0;
532  raw_torque_is_valid = (in[7] & 0x02) > 0;
533  rotation_rate_is_valid = (in[7] & 0x04) > 0;
534  user_interaction_is_valid = (in[7] & 0x08) > 0;
535 }
536 
537 void SteeringPIDRpt1Msg::parse(uint8_t *in)
538 {
539  int16_t temp;
540 
541  temp = ((int16_t)in[0] << 8) | in[1];
542  dt = (double)(temp / 1000.0);
543 
544  temp = ((int16_t)in[2] << 8) | in[3];
545  Kp = (double)(temp / 1000.0);
546 
547  temp = ((int16_t)in[4] << 8) | in[5];
548  Ki = (double)(temp / 1000.0);
549 
550  temp = ((int16_t)in[6] << 8) | in[7];
551  Kd = (double)(temp / 1000.0);
552 }
553 
554 void SteeringPIDRpt2Msg::parse(uint8_t *in)
555 {
556  int16_t temp;
557 
558  temp = ((int16_t)in[0] << 8) | in[1];
559  P_term = (double)(temp / 1000.0);
560 
561  temp = ((int16_t)in[2] << 8) | in[3];
562  I_term = (double)(temp / 1000.0);
563 
564  temp = ((int16_t)in[4] << 8) | in[5];
565  D_term = (double)(temp / 1000.0);
566 
567  temp = ((int16_t)in[6] << 8) | in[7];
568  all_terms = (double)(temp / 1000.0);
569 }
570 
571 void SteeringPIDRpt3Msg::parse(uint8_t *in)
572 {
573  int16_t temp;
574 
575  temp = ((int16_t)in[0] << 8) | in[1];
576  new_torque = (double)(temp / 1000.0);
577 
578  temp = ((int16_t)in[2] << 8) | in[3];
579  str_angle_desired = (double)(temp / 1000.0);
580 
581  temp = ((int16_t)in[4] << 8) | in[5];
582  str_angle_actual = (double)(temp / 1000.0);
583 
584  temp = ((int16_t)in[6] << 8) | in[7];
585  error = (double)(temp / 1000.0);
586 }
587 
588 void SteeringPIDRpt4Msg::parse(uint8_t *in)
589 {
590  int16_t temp;
591 
592  temp = ((int16_t)in[0] << 8) | in[1];
593  angular_velocity = (double)(temp / 1000.0);
594 
595  temp = ((int16_t)in[2] << 8) | in[3];
596  angular_acceleration = (double)(temp / 1000.0);
597 }
598 
599 void TurnAuxRptMsg::parse(uint8_t *in)
600 {
601  driver_blinker_bulb_on = (in[0] & 0x01) > 0;
602  passenger_blinker_bulb_on = (in[0] & 0x02) > 0;
603  driver_blinker_bulb_on_is_valid = (in[1] & 0x01) > 0;
604  passenger_blinker_bulb_on_is_valid = (in[1] & 0x02) > 0;
605 }
606 
607 void VehicleSpecificRpt1Msg::parse(uint8_t *in)
608 {
609  shift_pos_1 = in[0];
610  shift_pos_2 = in[1];
611 }
612 
613 void VehicleDynamicsRptMsg::parse(uint8_t *in)
614 {
615  int16_t temp;
616 
617  temp = (((int16_t)in[1] << 8) | in[2]);
618  brake_torque = (double)(temp / 1000.0);
619 
620  g_forces = in[0];
621 }
622 
623 void VehicleSpeedRptMsg::parse(uint8_t *in)
624 {
625  int16_t temp;
626 
627  temp = ((int16_t)in[0] << 8) | in[1];
628  vehicle_speed = (double)(temp / 100.0);
629 
630  vehicle_speed_valid = (in[2] == 1);
631  vehicle_speed_raw[0] = in[3];
632  vehicle_speed_raw[1] = in[4];
633 }
634 
635 void VinRptMsg::parse(uint8_t *in)
636 {
637  std::ostringstream oss;
638  oss << in[0] << in[1] << in[2];
639  mfg_code = oss.str();
640 
641  if (mfg_code == "52C")
642  mfg = "POLARIS INDUSTRIES INC.";
643  else if (mfg_code == "3HS")
644  mfg = "NAVISTAR, INC.";
645  else if (mfg_code == "2T2")
646  mfg = "TOYOTA MOTOR MANUFACTURING CANADA";
647  else
648  mfg = "UNKNOWN";
649 
650  model_year_code = in[3];
651 
652  if (model_year_code >= '1' && model_year_code <= '9')
653  {
654  model_year = 2000 + model_year_code;
655  }
656  else if (model_year_code >= 'A' && model_year_code < 'Z')
657  {
658  switch (model_year_code)
659  {
660  case 'A':
661  model_year = 2010;
662  break;
663  case 'B':
664  model_year = 2011;
665  break;
666  case 'C':
667  model_year = 2012;
668  break;
669  case 'D':
670  model_year = 2013;
671  break;
672  case 'E':
673  model_year = 2014;
674  break;
675  case 'F':
676  model_year = 2015;
677  break;
678  case 'G':
679  model_year = 2016;
680  break;
681  case 'H':
682  model_year = 2017;
683  break;
684  case 'J':
685  model_year = 2018;
686  break;
687  case 'K':
688  model_year = 2019;
689  break;
690  case 'L':
691  model_year = 2020;
692  break;
693  case 'M':
694  model_year = 2021;
695  break;
696  case 'N':
697  model_year = 2022;
698  break;
699  case 'P':
700  model_year = 2023;
701  break;
702  case 'R':
703  model_year = 2024;
704  break;
705  case 'S':
706  model_year = 2025;
707  break;
708  case 'T':
709  model_year = 2026;
710  break;
711  case 'V':
712  model_year = 2027;
713  break;
714  case 'W':
715  model_year = 2028;
716  break;
717  case 'X':
718  model_year = 2029;
719  break;
720  case 'Y':
721  model_year = 2030;
722  break;
723  default:
724  model_year = 9999;
725  }
726  }
727  else
728  {
729  model_year = 9999;
730  }
731 
732  serial = (in[4] & 0x0F);
733  serial = (serial << 8) | in[5];
734  serial = (serial << 8) | in[6];
735 }
736 
737 void WheelSpeedRptMsg::parse(uint8_t *in)
738 {
739  int16_t temp;
740 
741  temp = ((int16_t)in[0] << 8) | in[1];
742  front_left_wheel_speed = (double)(temp / 100.0);
743 
744  temp = ((int16_t)in[2] << 8) | in[3];
745  front_right_wheel_speed = (double)(temp / 100.0);
746 
747  temp = ((int16_t)in[4] << 8) | in[5];
748  rear_left_wheel_speed = (double)(temp / 100.0);
749 
750  temp = ((int16_t)in[6] << 8) | in[7];
751  rear_right_wheel_speed = (double)(temp / 100.0);
752 }
753 
754 void WiperAuxRptMsg::parse(uint8_t *in)
755 {
756  front_wiping = (in[0] & 0x01) > 0;
757  front_spraying = (in[0] & 0x02) > 0;
758  rear_wiping = (in[0] & 0x04) > 0;
759  rear_spraying = (in[0] & 0x08) > 0;
760  spray_near_empty = (in[0] & 0x10) > 0;
761  spray_empty = (in[0] & 0x20) > 0;
762  front_wiping_is_valid = (in[1] & 0x01) > 0;
763  front_spraying_is_valid = (in[1] & 0x02) > 0;
764  rear_wiping_is_valid = (in[1] & 0x04) > 0;
765  rear_spraying_is_valid = (in[1] & 0x08) > 0;
766  spray_near_empty_is_valid = (in[1] & 0x10) > 0;
767  spray_empty_is_valid = (in[1] & 0x20) > 0;
768 }
769 
770 void YawRateRptMsg::parse(uint8_t *in)
771 {
772  int16_t temp;
773 
774  temp = ((int16_t)in[0] << 8) | in[1];
775  yaw_rate = (double)(temp / 100.0);
776 }
777 
778 // RX Messages
779 void SystemCmdBool::encode(bool enable,
780  bool ignore_overrides,
781  bool clear_override,
782  bool cmd)
783 {
784  data.assign(8, 0);
785 
786  data[0] = (enable ? 0x01 : 0x00);
787  data[0] |= (ignore_overrides ? 0x02 : 0x00);
788  data[0] |= clear_override ? 0x04 : 0x00;
789  data[1] = (cmd ? 0x01 : 0x00);
790 }
791 
792 void SystemCmdFloat::encode(bool enable,
793  bool ignore_overrides,
794  bool clear_override,
795  float cmd)
796 {
797  data.assign(8, 0);
798 
799  data[0] = enable ? 0x01 : 0x00;
800  data[0] |= ignore_overrides ? 0x02 : 0x00;
801  data[0] |= clear_override ? 0x04 : 0x00;
802 
803  uint16_t cmd_float = (uint16_t)(cmd * 1000.0);
804  data[1] = (cmd_float & 0xFF00) >> 8;
805  data[2] = cmd_float & 0x00FF;
806 }
807 
808 void SystemCmdInt::encode(bool enable,
809  bool ignore_overrides,
810  bool clear_override,
811  uint8_t cmd)
812 {
813  data.assign(8, 0);
814 
815  data[0] = enable ? 0x01 : 0x00;
816  data[0] |= ignore_overrides ? 0x02 : 0x00;
817  data[0] |= clear_override ? 0x04 : 0x00;
818  data[1] = cmd;
819 }
820 
821 void SteerCmdMsg::encode(bool enable,
822  bool ignore_overrides,
823  bool clear_override,
824  float steer_pos,
825  float steer_spd)
826 {
827  data.assign(8, 0);
828 
829  data[0] = enable ? 0x01 : 0x00;
830  data[0] |= ignore_overrides ? 0x02 : 0x00;
831  data[0] |= clear_override ? 0x04 : 0x00;
832 
833  int16_t raw_pos = (int16_t)(1000.0 * steer_pos);
834  uint16_t raw_spd = (uint16_t)(1000.0 * steer_spd);
835 
836  data[1] = (raw_pos & 0xFF00) >> 8;
837  data[2] = raw_pos & 0x00FF;
838  data[3] = (raw_spd & 0xFF00) >> 8;
839  data[4] = raw_spd & 0x00FF;
840 }
AS::Drivers::PACMod3::SystemCmdInt::encode
void encode(bool enable, bool ignore_overrides, bool clear_override, uint8_t cmd)
Definition: pacmod3_core.cpp:808
AS::Drivers::PACMod3::Pacmod3TxMsg::make_message
static std::shared_ptr< Pacmod3TxMsg > make_message(const int64_t &can_id)
Definition: pacmod3_core.cpp:80
AS::Drivers::PACMod3::HornCmdMsg::CAN_ID
static const int64_t CAN_ID
Definition: pacmod3_core.h:779
pacmod3_core.h
AS::Drivers::PACMod3::SteerCmdMsg::encode
void encode(bool enabled, bool ignore_overrides, bool clear_override, float steer_pos, float steer_spd)
Definition: pacmod3_core.cpp:821
AS::Drivers::PACMod3::DoorRptMsg::CAN_ID
static const int64_t CAN_ID
Definition: pacmod3_core.h:488
command
ROSLIB_DECL std::string command(const std::string &cmd)
AS::Drivers::PACMod3::VinRptMsg::CAN_ID
static const int64_t CAN_ID
Definition: pacmod3_core.h:617
AS::Drivers::PACMod3::SystemCmdFloat::encode
void encode(bool enable, bool ignore_overrides, bool clear_override, float cmd)
Definition: pacmod3_core.cpp:792
AS::Drivers::PACMod3::SystemCmdBool::encode
void encode(bool enable, bool ignore_overrides, bool clear_override, bool cmd)
Definition: pacmod3_core.cpp:779
AS::Drivers::PACMod3::HornRptMsg::CAN_ID
static const int64_t CAN_ID
Definition: pacmod3_core.h:189

pacmod3
Author(s): Joe Driscoll , Josh Whitley
autogenerated on Mon Jun 10 2019 14:09:46