Message_cmd.cpp
Go to the documentation of this file.
1 
47 
48 #include <cstring>
49 #include <iostream>
50 #include <typeinfo>
51 
52 using namespace std;
53 
55 // Conditions on the below to handle compiling for nonstandard hardware
56 #ifdef LOGGING_AVAIL
58 #endif
59 
60 namespace clearpath
61 {
62 
63  long CmdMessage::total_destroyed = 0;
64  long CmdMessage::total_sent = 0;
65 
66  CmdMessage::~CmdMessage()
67  {
68  ++total_destroyed;
69  if (!is_sent)
70  {
71 #ifdef LOGGING_AVAIL
72  CPR_WARN() << "Command message destroyed without being sent. Type: "
73  << "0x" << hex << getType() << dec
74  << ". Total unsent: " << (total_destroyed-total_sent) << endl;
75 #endif
76  }
77  else
78  {
79  total_sent++;
80  }
81  }
82 
83 
84  CmdProcessorReset::CmdProcessorReset() : CmdMessage()
85  {
87  utob(getPayloadPointer(), 2, (uint16_t) 0x3A18);
89  makeValid();
90  }
91 
93  {
94  }
95 
96 
98  {
100 
101  utob(getPayloadPointer(), 2, (uint16_t) (0x3a18));
102  *getPayloadPointer(2) = (uint8_t) (flags);
104 
105  makeValid();
106  }
107 
109  {
110  }
111 
112 
114  {
115  setPayloadLength(2);
116 
117  utob(getPayloadPointer(), 2, (uint16_t) (0x3a18));
119 
120  makeValid();
121  }
122 
124  {
125  }
126 
127 
128  SetAckermannOutput::SetAckermannOutput(double steer, double throt, double brake) : CmdMessage()
129  {
131 
132  ftob(getPayloadPointer(STEERING), 2, steer, 100);
133  ftob(getPayloadPointer(THROTTLE), 2, throt, 100);
134  ftob(getPayloadPointer(BRAKE), 2, brake, 100);
135 
137  makeValid();
138  }
139 
141  {
142  }
143 
144 
146  double p,
147  double i,
148  double d,
149  double feedfwd,
150  double stic,
151  double int_lim)
152  : CmdMessage()
153  {
155 
156  ftob(getPayloadPointer(LEFT_P), 2, p, 100);
157  ftob(getPayloadPointer(LEFT_I), 2, i, 100);
158  ftob(getPayloadPointer(LEFT_D), 2, d, 100);
159  ftob(getPayloadPointer(LEFT_FEEDFWD), 2, feedfwd, 100);
160  ftob(getPayloadPointer(LEFT_STIC), 2, stic, 100);
161  ftob(getPayloadPointer(LEFT_INT_LIM), 2, int_lim, 100);
162 
163  ftob(getPayloadPointer(RIGHT_P), 2, p, 100);
164  ftob(getPayloadPointer(RIGHT_I), 2, i, 100);
165  ftob(getPayloadPointer(RIGHT_D), 2, d, 100);
166  ftob(getPayloadPointer(RIGHT_FEEDFWD), 2, feedfwd, 100);
167  ftob(getPayloadPointer(RIGHT_STIC), 2, stic, 100);
168  ftob(getPayloadPointer(RIGHT_INT_LIM), 2, int_lim, 100);
169 
171  makeValid();
172  }
173 
175  double left_p,
176  double left_i,
177  double left_d,
178  double left_feedfwd,
179  double left_stic,
180  double left_int_lim,
181  double right_p,
182  double right_i,
183  double right_d,
184  double right_feedfwd,
185  double right_stic,
186  double right_int_lim)
187  : CmdMessage()
188  {
190 
191  ftob(getPayloadPointer(LEFT_P), 2, left_p, 100);
192  ftob(getPayloadPointer(LEFT_I), 2, left_i, 100);
193  ftob(getPayloadPointer(LEFT_D), 2, left_d, 100);
194  ftob(getPayloadPointer(LEFT_FEEDFWD), 2, left_feedfwd, 100);
195  ftob(getPayloadPointer(LEFT_STIC), 2, left_stic, 100);
196  ftob(getPayloadPointer(LEFT_INT_LIM), 2, left_int_lim, 100);
197 
198  ftob(getPayloadPointer(RIGHT_P), 2, right_p, 100);
199  ftob(getPayloadPointer(RIGHT_I), 2, right_i, 100);
200  ftob(getPayloadPointer(RIGHT_D), 2, right_d, 100);
201  ftob(getPayloadPointer(RIGHT_FEEDFWD), 2, right_feedfwd, 100);
202  ftob(getPayloadPointer(RIGHT_STIC), 2, right_stic, 100);
203  ftob(getPayloadPointer(RIGHT_INT_LIM), 2, right_int_lim, 100);
204 
206  makeValid();
207  }
208 
210  : CmdMessage(other)
211  {
212  }
213 
214 
216  {
218  ftob(getPayloadPointer(LEFT), 2, left, 100);
219  ftob(getPayloadPointer(RIGHT), 2, right, 100);
220 
222  makeValid();
223  }
224 
226  : CmdMessage(other)
227  {
228  }
229 
230 
231  SetDifferentialSpeed::SetDifferentialSpeed(double left_speed, double right_speed,
232  double left_accel, double right_accel)
233  : CmdMessage()
234  {
236 
237  ftob(getPayloadPointer(LEFT_SPEED), 2, left_speed, 100);
238  ftob(getPayloadPointer(LEFT_ACCEL), 2, left_accel, 100);
239  ftob(getPayloadPointer(RIGHT_SPEED), 2, right_speed, 100);
240  ftob(getPayloadPointer(RIGHT_ACCEL), 2, right_accel, 100);
241 
243  makeValid();
244  }
245 
247  {
248  }
249 
250 
251  SetGear::SetGear(uint8_t gear) : CmdMessage()
252  {
253  setPayloadLength(1);
254  getPayloadPointer()[0] = gear;
256  makeValid();
257  }
258 
259  SetGear::SetGear(const SetGear &other) : CmdMessage(other)
260  {
261  }
262 
263 
264  SetMaxAccel::SetMaxAccel(double max_fwd, double max_rev) : CmdMessage()
265  {
267 
268  ftob(getPayloadPointer(MAX_FWD), 2, max_fwd, 100);
269  ftob(getPayloadPointer(MAX_REV), 2, max_rev, 100);
270 
272  makeValid();
273  }
274 
276  {
277  }
278 
279 
280  SetMaxSpeed::SetMaxSpeed(double max_fwd, double max_rev) : CmdMessage()
281  {
283 
284  ftob(getPayloadPointer(MAX_FWD), 2, max_fwd, 100);
285  ftob(getPayloadPointer(MAX_REV), 2, max_rev, 100);
286 
288  makeValid();
289  }
290 
292  {
293  }
294 
295 
297  {
298  size_t cpy_len = strlen(name);
299  size_t max_len = MAX_MSG_LENGTH - HEADER_LENGTH - CRC_LENGTH - 1 /* for size field */;
300  if (cpy_len > max_len) { cpy_len = max_len; }
301 
302  setPayloadLength(cpy_len + 1);
303  getPayloadPointer()[0] = cpy_len;
304  memcpy(getPayloadPointer(1), name, cpy_len);
305 
307 
308  makeValid();
309  }
310 
312  {
313  }
314 
315 
317  {
318  setPayloadLength(4);
319  utob(getPayloadPointer(), 4, time);
321  makeValid();
322  }
323 
325  {
326  }
327 
328 
330  {
331  setPayloadLength(2);
332  utob(getPayloadPointer(), 2, flags);
334  makeValid();
335  }
336 
338  {
339  }
340 
341 
342  SetTurn::SetTurn(double trans, double rad, double accel) : CmdMessage()
343  {
345 
346  ftob(getPayloadPointer(TRANSLATIONAL), 2, trans, 100);
347  ftob(getPayloadPointer(TURN_RADIUS), 2, rad, 100);
348  ftob(getPayloadPointer(TRANS_ACCEL), 2, accel, 100);
349 
351  makeValid();
352  }
353 
354  SetTurn::SetTurn(const SetTurn &other) : CmdMessage(other)
355  {
356  }
357 
358 
359  SetVelocity::SetVelocity(double trans, double rot, double accel) : CmdMessage()
360  {
362 
363  ftob(getPayloadPointer(TRANSLATIONAL), 2, trans, 100);
364  ftob(getPayloadPointer(ROTATIONAL), 2, rot, 100);
365  ftob(getPayloadPointer(TRANS_ACCEL), 2, accel, 100);
366 
368  makeValid();
369  }
370 
372  {
373  }
374 
375 
376 }; // namespace clearpath
377 
void utob(void *dest, size_t dest_len, uint64_t src)
Definition: Number.cpp:56
SetDifferentialSpeed(double left_spd, double right_speed, double left_accel, double right_accel)
SetPlatformName(const char *name)
SetMaxAccel(double max_fwd, double max_rev)
SetGear(uint8_t gear)
SetDifferentialOutput(double left, double right)
void setType(uint16_t type)
Definition: Message.cpp:245
void ftob(void *dest, size_t dest_len, double src, double scale)
Definition: Number.cpp:93
SetVelocity(double trans, double rot, double accel)
SetMaxSpeed(double max_fwd, double max_rev)
SetTurn(double trans, double rad, double accel)
SetSafetySystem(uint16_t flags)
uint8_t * getPayloadPointer(size_t offset=0)
Definition: Message.cpp:181
static const size_t CRC_LENGTH
Definition: Message.h:82
SetAckermannOutput(double steering, double throt, double brake)
SetPlatformTime(uint32_t time)
static const size_t MAX_MSG_LENGTH
Definition: Message.h:78
void setPayloadLength(uint8_t len)
Definition: Message.cpp:256
#define CPR_WARN()
Definition: Logger.h:107
SetDifferentialControl(double p, double i, double d, double feedfwd, double stic, double int_lim)
static const size_t HEADER_LENGTH
Definition: Message.h:85
CmdRestoreSettings(enum restoreFlags flags)
Definition: Message_cmd.cpp:97


husky_base
Author(s): Mike Purvis , Paul Bovbel
autogenerated on Fri Oct 2 2020 03:40:07