pacmod_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 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 <pacmod_core.h>
9 
10 using namespace AS::Drivers::PACMod;
11 
14 const int64_t AS::Drivers::PACMod::ShiftCmdMsg::CAN_ID = 0x65;
15 const int64_t AS::Drivers::PACMod::ShiftRptMsg::CAN_ID = 0x66;
16 const int64_t AS::Drivers::PACMod::AccelCmdMsg::CAN_ID = 0x67;
17 const int64_t AS::Drivers::PACMod::AccelRptMsg::CAN_ID = 0x68;
20 const int64_t AS::Drivers::PACMod::BrakeCmdMsg::CAN_ID = 0x6B;
21 const int64_t AS::Drivers::PACMod::SteerCmdMsg::CAN_ID = 0x6D;
22 const int64_t AS::Drivers::PACMod::BrakeRptMsg::CAN_ID = 0x6C;
23 const int64_t AS::Drivers::PACMod::SteerRptMsg::CAN_ID = 0x6E;
33 const int64_t AS::Drivers::PACMod::HornCmdMsg::CAN_ID = 0x78;
34 const int64_t AS::Drivers::PACMod::HornRptMsg::CAN_ID = 0x79;
46 const int64_t AS::Drivers::PACMod::WiperCmdMsg::CAN_ID = 0x90;
47 const int64_t AS::Drivers::PACMod::WiperRptMsg::CAN_ID = 0x91;
48 const int64_t AS::Drivers::PACMod::VinRptMsg::CAN_ID = 0xFF;
49 
50 std::shared_ptr<PacmodTxMsg> PacmodTxMsg::make_message(const int64_t& can_id)
51 {
52  switch (can_id)
53  {
55  return std::shared_ptr<PacmodTxMsg>(new TurnSignalRptMsg);
56  break;
58  return std::shared_ptr<PacmodTxMsg>(new ShiftRptMsg);
59  break;
61  return std::shared_ptr<PacmodTxMsg>(new AccelRptMsg);
62  break;
64  return std::shared_ptr<PacmodTxMsg>(new GlobalRptMsg);
65  break;
67  return std::shared_ptr<PacmodTxMsg>(new BrakeRptMsg);
68  break;
70  return std::shared_ptr<PacmodTxMsg>(new SteerRptMsg);
71  break;
73  return std::shared_ptr<PacmodTxMsg>(new VehicleSpeedRptMsg);
74  break;
76  return std::shared_ptr<PacmodTxMsg>(new BrakeMotorRpt1Msg);
77  break;
79  return std::shared_ptr<PacmodTxMsg>(new BrakeMotorRpt2Msg);
80  break;
82  return std::shared_ptr<PacmodTxMsg>(new BrakeMotorRpt3Msg);
83  break;
85  return std::shared_ptr<PacmodTxMsg>(new SteerMotorRpt1Msg);
86  break;
88  return std::shared_ptr<PacmodTxMsg>(new SteerMotorRpt2Msg);
89  break;
91  return std::shared_ptr<PacmodTxMsg>(new SteerMotorRpt3Msg);
92  break;
94  return std::shared_ptr<PacmodTxMsg>(new HeadlightRptMsg);
95  break;
96  case HornRptMsg::CAN_ID:
97  return std::shared_ptr<PacmodTxMsg>(new HornRptMsg);
98  break;
100  return std::shared_ptr<PacmodTxMsg>(new WheelSpeedRptMsg);
101  break;
103  return std::shared_ptr<PacmodTxMsg>(new SteeringPIDRpt1Msg);
104  break;
106  return std::shared_ptr<PacmodTxMsg>(new SteeringPIDRpt2Msg);
107  break;
109  return std::shared_ptr<PacmodTxMsg>(new SteeringPIDRpt3Msg);
110  break;
112  return std::shared_ptr<PacmodTxMsg>(new SteerRpt2Msg);
113  break;
115  return std::shared_ptr<PacmodTxMsg>(new SteerRpt3Msg);
116  break;
118  return std::shared_ptr<PacmodTxMsg>(new ParkingBrakeStatusRptMsg);
119  break;
121  return std::shared_ptr<PacmodTxMsg>(new YawRateRptMsg);
122  break;
124  return std::shared_ptr<PacmodTxMsg>(new LatLonHeadingRptMsg);
125  break;
127  return std::shared_ptr<PacmodTxMsg>(new DateTimeRptMsg);
128  break;
130  return std::shared_ptr<PacmodTxMsg>(new SteeringPIDRpt4Msg);
131  break;
132  case WiperRptMsg::CAN_ID:
133  return std::shared_ptr<PacmodTxMsg>(new WiperRptMsg);
134  break;
135  case VinRptMsg::CAN_ID:
136  return std::shared_ptr<PacmodTxMsg>(new VinRptMsg);
137  break;
138  default:
139  return NULL;
140  }
141 }
142 
143 // TX Messages
144 void GlobalRptMsg::parse(uint8_t *in)
145 {
146  enabled = in[0] & 0x01;
147  override_active = ((in[0] & 0x02) >> 1) != 0;
148  user_can_timeout = ((in[0] & 0x20) >> 5) != 0;
149  brake_can_timeout = ((in[0] & 0x10) >> 4) != 0;
150  steering_can_timeout = ((in[0] & 0x08) >> 3) != 0;
151  vehicle_can_timeout = ((in[0] & 0x04) >> 2) != 0;
152  user_can_read_errors = ((in[6] << 8) | in[7]);
153 }
154 
155 void VinRptMsg::parse(uint8_t *in)
156 {
157  std::ostringstream oss;
158  oss << in[0] << in[1] << in[2];
159  mfg_code = oss.str();
160 
161  if (mfg_code == "52C")
162  mfg = "POLARIS INDUSTRIES INC.";
163  else if (mfg_code == "3HS")
164  mfg = "NAVISTAR, INC.";
165  else if (mfg_code == "2T2")
166  mfg = "TOYOTA MOTOR MANUFACTURING CANADA";
167 
168  model_year_code = in[3];
169 
170  if (model_year_code >= '1' && model_year_code <= '9')
171  {
172  model_year = 2000 + model_year_code;
173  }
174  else if (model_year_code >= 'A' && model_year_code < 'Z')
175  {
176  switch (model_year_code)
177  {
178  case 'A':
179  model_year = 2010;
180  break;
181  case 'B':
182  model_year = 2011;
183  break;
184  case 'C':
185  model_year = 2012;
186  break;
187  case 'D':
188  model_year = 2013;
189  break;
190  case 'E':
191  model_year = 2014;
192  break;
193  case 'F':
194  model_year = 2015;
195  break;
196  case 'G':
197  model_year = 2016;
198  break;
199  case 'H':
200  model_year = 2017;
201  break;
202  case 'J':
203  model_year = 2018;
204  break;
205  case 'K':
206  model_year = 2019;
207  break;
208  case 'L':
209  model_year = 2020;
210  break;
211  case 'M':
212  model_year = 2021;
213  break;
214  case 'N':
215  model_year = 2022;
216  break;
217  case 'P':
218  model_year = 2023;
219  break;
220  case 'R':
221  model_year = 2024;
222  break;
223  case 'S':
224  model_year = 2025;
225  break;
226  case 'T':
227  model_year = 2026;
228  break;
229  case 'V':
230  model_year = 2027;
231  break;
232  case 'W':
233  model_year = 2028;
234  break;
235  case 'X':
236  model_year = 2029;
237  break;
238  case 'Y':
239  model_year = 2030;
240  break;
241  }
242  }
243 
244  serial = (in[4] & 0x0F);
245  serial = (serial << 8) | in[5];
246  serial = (serial << 8) | in[6];
247 }
248 
249 void SystemRptIntMsg::parse(uint8_t *in)
250 {
251  manual_input = in[0];
252  command = in[1];
253  output = in[2];
254 }
255 
256 void SystemRptFloatMsg::parse(uint8_t *in)
257 {
258  int16_t temp;
259 
260  temp = (static_cast<int16_t>(in[0]) << 8) | in[1];
261  manual_input = static_cast<double>(temp / 1000.0);
262 
263  temp = (static_cast<int16_t>(in[2]) << 8) | in[3];
264  command = static_cast<double>(temp / 1000.0);
265 
266  temp = (static_cast<int16_t>(in[4]) << 8) | in[5];
267  output = static_cast<double>(temp / 1000.0);
268 }
269 
270 void VehicleSpeedRptMsg::parse(uint8_t *in)
271 {
272  int16_t temp;
273 
274  temp = (static_cast<int16_t>(in[0]) << 8) | in[1];
275  vehicle_speed = static_cast<double>(temp / 100.0);
276 
277  vehicle_speed_valid = (in[2] == 1);
278  vehicle_speed_raw[0] = in[3];
279  vehicle_speed_raw[1] = in[4];
280 }
281 
282 void YawRateRptMsg::parse(uint8_t *in)
283 {
284  int16_t temp;
285 
286  temp = (static_cast<int16_t>(in[0]) << 8) | in[1];
287  yaw_rate = static_cast<double>(temp / 100.0);
288 }
289 
290 void LatLonHeadingRptMsg::parse(uint8_t *in)
291 {
292  latitude_degrees = static_cast<int8_t>(in[0]);
293  latitude_minutes = in[1];
294  latitude_seconds = in[2];
295  longitude_degrees = static_cast<int8_t>(in[3]);
296  longitude_minutes = in[4];
297  longitude_seconds = in[5];
298  heading = ((static_cast<int16_t>(in[6]) << 8) | in[7]) / 100.0;
299 }
300 
301 void DateTimeRptMsg::parse(uint8_t *in)
302 {
303  year = in[0];
304  month = in[1];
305  day = in[2];
306  hour = in[3];
307  minute = in[4];
308  second = in[5];
309 }
310 
311 void WheelSpeedRptMsg::parse(uint8_t *in)
312 {
313  int16_t temp;
314 
315  temp = (static_cast<int16_t>(in[0]) << 8) | in[1];
316  front_left_wheel_speed = static_cast<double>(temp / 100.0);
317 
318  temp = (static_cast<int16_t>(in[2]) << 8) | in[3];
319  front_right_wheel_speed = static_cast<double>(temp / 100.0);
320 
321  temp = (static_cast<int16_t>(in[4]) << 8) | in[5];
322  rear_left_wheel_speed = static_cast<double>(temp / 100.0);
323 
324  temp = (static_cast<int16_t>(in[6]) << 8) | in[7];
325  rear_right_wheel_speed = static_cast<double>(temp / 100.0);
326 }
327 
328 void MotorRpt1Msg::parse(uint8_t *in)
329 {
330  int32_t temp;
331 
332  temp = (static_cast<int32_t>(in[0]) << 24) |
333  (static_cast<int32_t>(in[1]) << 16) |
334  (static_cast<int32_t>(in[2]) << 8) | in[3];
335  current = static_cast<double>(temp / 1000.0);
336 
337  temp = (static_cast<int32_t>(in[4]) << 24) |
338  (static_cast<int32_t>(in[5]) << 16) |
339  (static_cast<int32_t>(in[6]) << 8) | in[7];
340  position = static_cast<double>(temp / 1000.0);
341 }
342 
343 void MotorRpt2Msg::parse(uint8_t *in)
344 {
345  int16_t temp16;
346  int32_t temp32;
347 
348  temp16 = (static_cast<int16_t>(in[0]) << 8) | in[1];
349  encoder_temp = static_cast<double>(temp16);
350 
351  temp16 = (static_cast<int16_t>(in[2]) << 8) | in[3];
352  motor_temp = static_cast<double>(temp16);
353 
354  temp32 = (static_cast<int32_t>(in[7]) << 24) |
355  (static_cast<int32_t>(in[6]) << 16) |
356  (static_cast<int32_t>(in[5]) << 8) | in[4];
357  velocity = static_cast<double>(temp32 / 1000.0);
358 }
359 
360 void MotorRpt3Msg::parse(uint8_t *in)
361 {
362  int32_t temp;
363 
364  temp = (static_cast<int32_t>(in[0]) << 24) |
365  (static_cast<int32_t>(in[1]) << 16) |
366  (static_cast<int32_t>(in[2]) << 8) | in[3];
367  torque_output = static_cast<double>(temp / 1000.0);
368 
369  temp = (static_cast<int32_t>(in[4]) << 24) |
370  (static_cast<int32_t>(in[5]) << 16) |
371  (static_cast<int32_t>(in[6]) << 8) | in[7];
372  torque_input = static_cast<double>(temp / 1000.0);
373 }
374 
375 void SteeringPIDRpt1Msg::parse(uint8_t *in)
376 {
377  int16_t temp;
378 
379  temp = (static_cast<int16_t>(in[0]) << 8) | in[1];
380  dt = static_cast<double>(temp / 1000.0);
381 
382  temp = (static_cast<int16_t>(in[2]) << 8) | in[3];
383  Kp = static_cast<double>(temp / 1000.0);
384 
385  temp = (static_cast<int16_t>(in[4]) << 8) | in[5];
386  Ki = static_cast<double>(temp / 1000.0);
387 
388  temp = (static_cast<int16_t>(in[6]) << 8) | in[7];
389  Kd = static_cast<double>(temp / 1000.0);
390 }
391 
392 void SteeringPIDRpt2Msg::parse(uint8_t *in)
393 {
394  int16_t temp;
395 
396  temp = (static_cast<int16_t>(in[0]) << 8) | in[1];
397  P_term = static_cast<double>(temp / 1000.0);
398 
399  temp = (static_cast<int16_t>(in[2]) << 8) | in[3];
400  I_term = static_cast<double>(temp / 1000.0);
401 
402  temp = (static_cast<int16_t>(in[4]) << 8) | in[5];
403  D_term = static_cast<double>(temp / 1000.0);
404 
405  temp = (static_cast<int16_t>(in[6]) << 8) | in[7];
406  all_terms = static_cast<double>(temp / 1000.0);
407 }
408 
409 void SteeringPIDRpt3Msg::parse(uint8_t *in)
410 {
411  int16_t temp;
412 
413  temp = (static_cast<int16_t>(in[0]) << 8) | in[1];
414  new_torque = static_cast<double>(temp / 1000.0);
415 
416  temp = (static_cast<int16_t>(in[2]) << 8) | in[3];
417  str_angle_desired = static_cast<double>(temp / 1000.0);
418 
419  temp = (static_cast<int16_t>(in[4]) << 8) | in[5];
420  str_angle_actual = static_cast<double>(temp / 1000.0);
421 
422  temp = (static_cast<int16_t>(in[6]) << 8) | in[7];
423  error = static_cast<double>(temp / 1000.0);
424 }
425 
426 void SteeringPIDRpt4Msg::parse(uint8_t *in)
427 {
428  int16_t temp;
429 
430  temp = (static_cast<int16_t>(in[0]) << 8) | in[1];
431  angular_velocity = static_cast<double>(temp / 1000.0);
432 
433  temp = (static_cast<int16_t>(in[2]) << 8) | in[3];
434  angular_acceleration = static_cast<double>(temp / 1000.0);
435 }
436 
438 {
439  parking_brake_engaged = (in[0] == 0x01);
440 }
441 
442 // RX Messages
443 void GlobalCmdMsg::encode(bool enable, bool clear_override, bool ignore_overide)
444 {
445  data.assign(8, 0);
446 
447  if (enable)
448  data[0] |= 0x01;
449  if (clear_override)
450  data[0] |= 0x02;
451  if (ignore_overide)
452  data[0] |= 0x04;
453 }
454 
455 void TurnSignalCmdMsg::encode(uint8_t turn_signal_cmd)
456 {
457  data.assign(8, 0);
458  data[0] = turn_signal_cmd;
459 }
460 
461 void HeadlightCmdMsg::encode(uint8_t headlight_cmd)
462 {
463  data.assign(8, 0);
464  data[0] = headlight_cmd;
465 }
466 
467 void HornCmdMsg::encode(uint8_t horn_cmd)
468 {
469  data.assign(8, 0);
470  data[0] = horn_cmd;
471 }
472 
473 void WiperCmdMsg::encode(uint8_t wiper_cmd)
474 {
475  data.assign(8, 0);
476  data[0] = wiper_cmd;
477 }
478 
479 void ShiftCmdMsg::encode(uint8_t shift_cmd)
480 {
481  data.assign(8, 0);
482  data[0] = shift_cmd;
483 }
484 
485 void AccelCmdMsg::encode(double accel_cmd)
486 {
487  data.assign(8, 0);
488  uint16_t cmdInt = static_cast<uint16_t>(accel_cmd * 1000.0);
489  data[0] = (cmdInt & 0xFF00) >> 8;
490  data[1] = cmdInt & 0x00FF;
491 }
492 
493 void SteerCmdMsg::encode(double steer_pos, double steer_spd)
494 {
495  data.assign(8, 0);
496  int32_t raw_pos = static_cast<int32_t>(1000.0 * steer_pos);
497  uint32_t raw_spd = (uint32_t)(1000.0 * steer_spd);
498 
499  data[0] = (raw_pos & 0xFF000000) >> 24;
500  data[1] = (raw_pos & 0x00FF0000) >> 16;
501  data[2] = (raw_pos & 0x0000FF00) >> 8;
502  data[3] = raw_pos & 0x000000FF;
503  data[4] = (raw_spd & 0xFF000000) >> 24;
504  data[5] = (raw_spd & 0x00FF0000) >> 16;
505  data[6] = (raw_spd & 0x0000FF00) >> 8;
506  data[7] = raw_spd & 0x000000FF;
507 }
508 
509 void BrakeCmdMsg::encode(double brake_pct)
510 {
511  data.assign(8, 0);
512  uint16_t raw_pct = static_cast<uint16_t>(1000.0 * brake_pct);
513 
514  data[0] = (raw_pct & 0xFF00) >> 8;
515  data[1] = (raw_pct & 0x00FF);
516 }
static const int64_t CAN_ID
Definition: pacmod_core.h:455
void encode(uint8_t horn_cmd)
static const int64_t CAN_ID
Definition: pacmod_core.h:114
static const int64_t CAN_ID
Definition: pacmod_core.h:410
static const int64_t CAN_ID
Definition: pacmod_core.h:437
static const int64_t CAN_ID
Definition: pacmod_core.h:153
void encode(double steer_pos, double steer_spd)
static const int64_t CAN_ID
Definition: pacmod_core.h:383
static const int64_t CAN_ID
Definition: pacmod_core.h:107
static const int64_t CAN_ID
Definition: pacmod_core.h:60
static const int64_t CAN_ID
Definition: pacmod_core.h:146
void encode(double accel_cmd)
void encode(uint8_t wiper_cmd)
void encode(double brake_cmd)
void encode(uint8_t turn_signal_cmd)
void encode(uint8_t shift_cmd)
static const int64_t CAN_ID
Definition: pacmod_core.h:419
static const int64_t CAN_ID
Definition: pacmod_core.h:100
ROSLIB_DECL std::string command(const std::string &cmd)
static const int64_t CAN_ID
Definition: pacmod_core.h:428
void encode(uint8_t headlight_cmd)
static const int64_t CAN_ID
Definition: pacmod_core.h:43
static const int64_t CAN_ID
Definition: pacmod_core.h:132
static const int64_t CAN_ID
Definition: pacmod_core.h:160
static const int64_t CAN_ID
Definition: pacmod_core.h:139
static std::shared_ptr< PacmodTxMsg > make_message(const int64_t &can_id)
Definition: pacmod_core.cpp:50
static const int64_t CAN_ID
Definition: pacmod_core.h:446
void encode(bool enable, bool clear_override, bool ignore_overide)


pacmod
Author(s): Joe Driscoll , Josh Whitley
autogenerated on Fri Jun 7 2019 21:54:13