comm_manager.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
3  *
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright notice, this
10  * list of conditions and the following disclaimer.
11  *
12  * * Redistributions in binary form must reproduce the above copyright notice,
13  * this list of conditions and the following disclaimer in the documentation
14  * and/or other materials provided with the distribution.
15  *
16  * * Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include <stdint.h>
33 
34 #include "rosflight.h"
35 #include "comm_manager.h"
36 
37 namespace rosflight_firmware
38 {
39 
41  RF_(rf),
42  comm_link_(comm_link)
43 {
44  initialized_ = false;
45  //instance = this;
46 }
47 
48 // function definitions
50 {
51  comm_link_.register_param_request_list_callback([this](uint8_t target_system)
52  {
53  this->param_request_list_callback(target_system);
54  });
55  comm_link_.register_param_request_read_callback([this](uint8_t target_system, const char *const param_name,
56  int16_t param_index)
57  {
58  this->param_request_read_callback(target_system, param_name, param_index);
59  });
60  comm_link_.register_param_set_int_callback([this](uint8_t target_system, const char *const param_name,
61  int32_t param_value)
62  {
63  this->param_set_int_callback(target_system, param_name, param_value);
64  });
65  comm_link_.register_param_set_float_callback([this](uint8_t target_system, const char *const param_name,
66  float param_value)
67  {
68  this->param_set_float_callback(target_system, param_name, param_value);
69  });
71  {
72  this->offboard_control_callback(control);
73  });
75  {
76  this->command_callback(command);
77  });
78  comm_link_.register_timesync_callback([this](int64_t tc1, int64_t ts1)
79  {
80  this->timesync_callback(tc1, ts1);
81  });
83  {
85  });
87  {
88  this->heartbeat_callback();
89  });
90  comm_link_.init(static_cast<uint32_t>(RF_.params_.get_param_int(PARAM_BAUD_RATE)),
91  static_cast<uint32_t>(RF_.params_.get_param_int(PARAM_SERIAL_DEVICE)));
92 
93  sysid_ = static_cast<uint8_t>(RF_.params_.get_param_int(PARAM_SYSTEM_ID));
94 
97 
98  // Register Param change callbacks
99  RF_.params_.add_callback([this](int16_t param_id)
100  {
101  this->update_system_id(param_id);
102  }, PARAM_SYSTEM_ID);
103  RF_.params_.add_callback([this](int16_t param_id)
104  {
105  this->set_streaming_rate(STREAM_ID_HEARTBEAT, param_id);
107  RF_.params_.add_callback([this](int16_t param_id)
108  {
109  this->set_streaming_rate(STREAM_ID_STATUS, param_id);
111  RF_.params_.add_callback([this](int16_t param_id)
112  {
113  this->set_streaming_rate(STREAM_ID_IMU, param_id);
115  RF_.params_.add_callback([this](int16_t param_id)
116  {
117  this->set_streaming_rate(STREAM_ID_ATTITUDE, param_id);
119  RF_.params_.add_callback([this](int16_t param_id)
120  {
123  RF_.params_.add_callback([this](int16_t param_id)
124  {
125  this->set_streaming_rate(STREAM_ID_BARO, param_id);
127  RF_.params_.add_callback([this](int16_t param_id)
128  {
129  this->set_streaming_rate(STREAM_ID_SONAR, param_id);
131  RF_.params_.add_callback([this](int16_t param_id)
132  {
133  this->set_streaming_rate(STREAM_ID_GNSS, param_id);
135  RF_.params_.add_callback([this](int16_t param_id)
136  {
137  this->set_streaming_rate(STREAM_ID_GNSS_RAW, param_id);
139  RF_.params_.add_callback([this](int16_t param_id)
140  {
141  this->set_streaming_rate(STREAM_ID_MAG, param_id);
143  RF_.params_.add_callback([this](int16_t param_id)
144  {
147  RF_.params_.add_callback([this](int16_t param_id)
148  {
149  this->set_streaming_rate(STREAM_ID_RC_RAW, param_id);
151 
152  initialized_ = true;
154 }
155 
156 void CommManager::update_system_id(uint16_t param_id)
157 {
158  (void) param_id;
159  sysid_ = static_cast<uint8_t>(RF_.params_.get_param_int(PARAM_SYSTEM_ID));
160 }
161 
163 {
164  send_status();
165 }
166 
167 void CommManager::send_param_value(uint16_t param_id)
168 {
169  if (param_id < PARAMS_COUNT)
170  {
171  switch (RF_.params_.get_param_type(param_id))
172  {
173  case PARAM_TYPE_INT32:
175  param_id,
176  RF_.params_.get_param_name(param_id),
177  RF_.params_.get_param_int(param_id),
178  static_cast<uint16_t>(PARAMS_COUNT));
179  break;
180  case PARAM_TYPE_FLOAT:
182  param_id,
183  RF_.params_.get_param_name(param_id),
184  RF_.params_.get_param_float(param_id),
185  static_cast<uint16_t>(PARAMS_COUNT));
186  break;
187  default:
188  break;
189  }
190  }
191 }
192 
193 void CommManager::param_request_list_callback(uint8_t target_system)
194 {
195  if (target_system == sysid_)
196  send_params_index_ = 0;
197 }
198 
200 {
201  send_params_index_ = 0;
202 }
203 
204 void CommManager::param_request_read_callback(uint8_t target_system, const char *const param_name, int16_t param_index)
205 {
206  if (target_system == sysid_)
207  {
208  uint16_t id = (param_index < 0) ? RF_.params_.lookup_param_id(param_name) : static_cast<uint16_t>(param_index);
209 
210  if (id < PARAMS_COUNT)
211  send_param_value(id);
212  }
213 }
214 
215 void CommManager::param_set_int_callback(uint8_t target_system, const char *const param_name, int32_t param_value)
216 {
217  if (target_system == sysid_)
218  {
219  uint16_t id = RF_.params_.lookup_param_id(param_name);
220 
222  {
223  RF_.params_.set_param_int(id, param_value);
224  }
225  }
226 }
227 
228 void CommManager::param_set_float_callback(uint8_t target_system, const char *const param_name, float param_value)
229 {
230  if (target_system == sysid_)
231  {
232  uint16_t id = RF_.params_.lookup_param_id(param_name);
233 
235  {
236  RF_.params_.set_param_float(id, param_value);
237  }
238  }
239 }
240 
242 {
243  bool result;
244  bool reboot_flag = false;
245  bool reboot_to_bootloader_flag = false;
246 
247  // None of these actions can be performed if we are armed
249  {
250  result = false;
251  }
252  else
253  {
254  result = true;
255  switch (command)
256  {
258  result = RF_.params_.read();
259  break;
261  result = RF_.params_.write();
262  break;
265  break;
268  break;
271  break;
274  break;
277  break;
280  break;
282  reboot_flag = true;
283  break;
285  reboot_to_bootloader_flag = true;
286  break;
289  break;
290  }
291  }
292 
293  comm_link_.send_command_ack(sysid_, command, result);
294 
295  if (reboot_flag || reboot_to_bootloader_flag)
296  {
297  RF_.board_.clock_delay(20);
298  RF_.board_.board_reset(reboot_to_bootloader_flag);
299  }
301 }
302 
303 void CommManager::timesync_callback(int64_t tc1, int64_t ts1)
304 {
305  uint64_t now_us = RF_.board_.clock_micros();
306 
307  if (tc1 == 0) // check that this is a request, not a response
308  comm_link_.send_timesync(sysid_, static_cast<int64_t>(now_us)*1000, ts1);
309 }
310 
312 {
313  // put values into a new command struct
314  control_t new_offboard_command;
315  new_offboard_command.x.value = control.x.value;
316  new_offboard_command.y.value = control.y.value;
317  new_offboard_command.z.value = control.z.value;
318  new_offboard_command.F.value = control.F.value;
319 
320  // Move flags into standard message
321  new_offboard_command.x.active = control.x.valid;
322  new_offboard_command.y.active = control.y.valid;
323  new_offboard_command.z.active = control.z.valid;
324  new_offboard_command.F.active = control.F.valid;
325 
326  // translate modes into standard message
327  switch (control.mode)
328  {
330  new_offboard_command.x.type = PASSTHROUGH;
331  new_offboard_command.y.type = PASSTHROUGH;
332  new_offboard_command.z.type = PASSTHROUGH;
333  new_offboard_command.F.type = THROTTLE;
334  break;
336  new_offboard_command.x.type = RATE;
337  new_offboard_command.y.type = RATE;
338  new_offboard_command.z.type = RATE;
339  new_offboard_command.F.type = THROTTLE;
340  break;
342  new_offboard_command.x.type = ANGLE;
343  new_offboard_command.y.type = ANGLE;
344  new_offboard_command.z.type = RATE;
345  new_offboard_command.F.type = THROTTLE;
346  break;
347  }
348 
349  // Tell the command_manager that we have a new command we need to mux
350  new_offboard_command.stamp_ms = RF_.board_.clock_millis();
351  RF_.command_manager_.set_new_offboard_command(new_offboard_command);
352 }
353 
355 {
357 }
359 {
360  static bool error_data_sent = false;
361  if (!error_data_sent)
362  {
363  if (this->RF_.board_.has_backup_data())
364  {
365  this->send_error_data();
366  }
367  error_data_sent = true;
368  }
369  this->send_heartbeat();//respond to heartbeats with a heartbeat
370 }
371 
372 // function definitions
374 {
376 }
377 
378 void CommManager::log(CommLink::LogSeverity severity, const char *fmt, ...)
379 {
380  // Convert the format string to a raw char array
381  va_list args;
382  va_start(args, fmt);
383  char text[50];
385  va_end(args);
386  comm_link_.send_log_message(sysid_, severity, text);
387 }
388 
390 {
392 }
393 
395 {
396  if (!initialized_)
397  return;
398 
399  uint8_t control_mode = 0;
401  control_mode = MODE_PASS_THROUGH;
403  control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE;
404  else
406 
413  control_mode,
416 }
417 
418 
420 {
425 }
426 
428 {
429  turbomath::Vector acc, gyro;
430  uint64_t stamp_us;
431  RF_.sensors_.get_filtered_IMU(acc, gyro, stamp_us);
433  stamp_us,
434  acc,
435  gyro,
437 
438 }
439 
441 {
445 }
446 
448 {
449  // TODO better mechanism for retreiving RC (through RC module, not PWM-specific)
450  uint16_t channels[8] = { static_cast<uint16_t>(RF_.board_.rc_read(0)*1000),
451  static_cast<uint16_t>(RF_.board_.rc_read(1)*1000),
452  static_cast<uint16_t>(RF_.board_.rc_read(2)*1000),
453  static_cast<uint16_t>(RF_.board_.rc_read(3)*1000),
454  static_cast<uint16_t>(RF_.board_.rc_read(4)*1000),
455  static_cast<uint16_t>(RF_.board_.rc_read(5)*1000),
456  static_cast<uint16_t>(RF_.board_.rc_read(6)*1000),
457  static_cast<uint16_t>(RF_.board_.rc_read(7)*1000)
458  };
460 }
461 
463 {
465  {
470  }
471 }
472 
474 {
475  if (RF_.sensors_.data().baro_valid)
476  {
481  }
482 }
483 
485 {
487  {
489  0, // TODO set sensor type (sonar/lidar), use enum
491  8.0,
492  0.25);
493  }
494 }
495 
497 {
500 }
502 {
503  BackupData error_data = RF_.board_.get_backup_data();
504  comm_link_.send_error_data(sysid_, error_data);
505 }
506 
508 {
510  {
511  GNSSData gnss_data = RF_.sensors_.data().gnss_data;
512  if (gnss_data.time_of_week!=this->last_sent_gnss_tow)
513  {
514  this->last_sent_gnss_tow = gnss_data.time_of_week;
516  gnss_data.time_of_week,
517  gnss_data.fix_type,
518  gnss_data.time,
519  gnss_data.nanos,
520  gnss_data.lat,
521  gnss_data.lon,
522  gnss_data.height,
523  gnss_data.vel_n,
524  gnss_data.vel_e,
525  gnss_data.vel_d,
526  gnss_data.h_acc,
527  gnss_data.v_acc,
528  gnss_data.ecef.x,
529  gnss_data.ecef.y,
530  gnss_data.ecef.z,
531  gnss_data.ecef.p_acc,
532  gnss_data.ecef.vx,
533  gnss_data.ecef.vy,
534  gnss_data.ecef.vz,
535  gnss_data.ecef.s_acc,
536  gnss_data.rosflight_timestamp);
537  }
538  }
539 }
540 
542 {
544  {
546  if (raw.time_of_week != this->last_sent_gnss_raw_tow)
548  raw.time_of_week,
549  raw.year,
550  raw.month,
551  raw.day,
552  raw.hour,
553  raw.min,
554  raw.sec,
555  raw.valid,
556  raw.t_acc,
557  raw.nano,
558  raw.fix_type,
559  raw.num_sat,
560  raw.lon,
561  raw.lat,
562  raw.height,
563  raw.height_msl,
564  raw.h_acc,
565  raw.v_acc,
566  raw.vel_n,
567  raw.vel_e,
568  raw.vel_d,
569  raw.g_speed,
570  raw.head_mot,
571  raw.s_acc,
572  raw.head_acc,
573  raw.p_dop,
574  raw.rosflight_timestamp);
576  }
577 }
578 
580 {
581  send_next_param();
582 }
583 
584 // function definitions
586 {
587  uint64_t time_us = RF_.board_.clock_micros();
588  for (int i = 0; i < STREAM_COUNT; i++)
589  {
590  streams_[i].stream(time_us);
591  }
593 }
594 
595 void CommManager::set_streaming_rate(uint8_t stream_id, int16_t param_id)
596 {
597  streams_[stream_id].set_rate(RF_.params_.get_param_int(param_id));
598 }
599 
600 void CommManager::send_named_value_int(const char *const name, int32_t value)
601 {
603 }
604 
605 void CommManager::send_named_value_float(const char *const name, float value)
606 {
608 }
609 
611 {
613  {
614  send_param_value(static_cast<uint16_t>(send_params_index_));
616  }
617 }
618 
619 CommManager::Stream::Stream(uint32_t period_us, std::function<void(void)> send_function) :
620  period_us_(period_us),
621  next_time_us_(0),
622  send_function_(send_function)
623 {}
624 
625 void CommManager::Stream::stream(uint64_t now_us)
626 {
627  if (period_us_ > 0 && now_us >= next_time_us_)
628  {
629  // if you fall behind, skip messages
630  do
631  {
633  }
634  while (next_time_us_ < now_us);
635 
636  send_function_();
637  }
638 }
639 
640 void CommManager::Stream::set_rate(uint32_t rate_hz)
641 {
642  period_us_ = (rate_hz == 0) ? 0 : 1000000/rate_hz;
643 }
644 
645 //void Mavlink::mavlink_send_named_command_struct(const char *const name, control_t command_struct)
646 //{
647 // uint8_t control_mode;
648 // if (command_struct.x.type == RATE && command_struct.y.type == RATE)
649 // {
650 // control_mode = MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE;
651 // }
652 // else if (command_struct.x.type == ANGLE && command_struct.y.type == ANGLE)
653 // {
654 // if (command_struct.x.type == ALTITUDE)
655 // {
656 // control_mode = MODE_ROLL_PITCH_YAWRATE_ALTITUDE;
657 // }
658 // else
659 // {
660 // control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE;
661 // }
662 // }
663 // else
664 // {
665 // control_mode = MODE_PASS_THROUGH;
666 // }
667 // uint8_t ignore = !(command_struct.x.active) ||
668 // !(command_struct.y.active) << 1 ||
669 // !(command_struct.z.active) << 2 ||
670 // !(command_struct.F.active) << 3;
671 // mavlink_message_t msg;
672 // mavlink_msg_named_command_struct_pack(sysid, compid, &msg, name,
673 // control_mode,
674 // ignore,
675 // command_struct.x.value,
676 // command_struct.y.value,
677 // command_struct.z.value,
678 // command_struct.F.value);
679 // send_message(msg);
680 //}
681 
682 } // namespace rosflight_firmware
void command_callback(CommLink::Command command)
void set_streaming_rate(uint8_t stream_id, int16_t param_id)
virtual float rc_read(uint8_t channel)=0
#define GIT_VERSION_STRING
Definition: param.h:45
static volatile int16_t gyro[3]
Definition: drv_mpu6050.c:277
void param_request_list_callback(uint8_t target_system)
struct rosflight_firmware::GNSSData::@109 ecef
void update_system_id(uint16_t param_id)
uint16_t lookup_param_id(const char name[PARAMS_NAME_LENGTH])
Gets the id of a parameter from its name.
Definition: param.cpp:351
const control_t & combined_control() const
const float * get_outputs() const
Definition: mixer.h:237
bool start_imu_calibration(void)
Definition: sensors.cpp:236
void param_set_int_callback(uint8_t target_system, const char *const param_name, int32_t param_value)
void set_defaults(void)
Set all parameters to default values.
Definition: param.cpp:100
bool read(void)
Read parameter values from non-volatile memory.
Definition: param.cpp:314
bool set_param_float(uint16_t id, float value)
Sets the value of a floating point parameter by ID and calls the parameter callback.
Definition: param.cpp:389
void send_named_value_int(const char *const name, int32_t value)
void send_param_value(uint16_t param_id)
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:326
const State & state() const
Definition: estimator.h:62
void set_attitude_correction(const turbomath::Quaternion &q)
Definition: estimator.cpp:115
virtual void serial_flush()=0
Stream(uint32_t period_us, std::function< void(void)> send_function)
ROSLIB_DECL std::string command(const std::string &cmd)
virtual void board_reset(bool bootloader)=0
static const channelConfig_t channels[CC_CHANNELS_PER_TIMER]
Definition: drv_timer.c:111
param_type_t get_param_type(uint16_t id) const
Get the type of a parameter.
Definition: param.h:349
void add_callback(std::function< void(int)> callback, uint16_t param_id)
Definition: param.cpp:308
bool start_gyro_calibration(void)
Definition: sensors.cpp:247
bool start_baro_calibration(void)
Definition: sensors.cpp:256
virtual uint64_t clock_micros()=0
const State & state() const
Definition: state_manager.h:82
void set_new_offboard_command(control_t new_offboard_command)
void param_set_float_callback(uint8_t target_system, const char *const param_name, float param_value)
virtual uint16_t num_sensor_errors()=0
virtual void clock_delay(uint32_t milliseconds)=0
CommManager(ROSflight &rf, CommLink &comm_link)
Stream streams_[STREAM_COUNT]
Definition: comm_manager.h:134
turbomath::Quaternion attitude
Definition: estimator.h:53
void log(CommLink::LogSeverity severity, const char *fmt,...)
void get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro, uint64_t &stamp_us)
Definition: sensors.cpp:343
const char * get_param_name(uint16_t id) const
Get the name of a parameter.
Definition: param.h:336
virtual uint32_t clock_millis()=0
turbomath::Vector angular_velocity
Definition: estimator.h:52
void timesync_callback(int64_t tc1, int64_t ts1)
int get_param_int(uint16_t id) const
Get the value of an integer parameter by id.
Definition: param.h:316
bool start_diff_pressure_calibration(void)
Definition: sensors.cpp:266
void param_request_read_callback(uint8_t target_system, const char *const param_name, int16_t param_index)
void tfp_sprintf(char *s, const char *fmt, va_list va)
Definition: nanoprintf.cpp:259
std::function< void(void)> send_function_
Definition: comm_manager.h:95
virtual bool has_backup_data()=0
void send_named_value_float(const char *const name, float value)
void offboard_control_callback(const CommLink::OffboardControl &control)
bool write(void)
Write current parameter values to non-volatile memory.
Definition: param.cpp:331
const Data & data() const
Definition: sensors.h:160
bool set_param_int(uint16_t id, int32_t value)
Sets the value of a parameter by ID and calls the parameter change callback.
Definition: param.cpp:377
uint8_t raw[15]
Definition: mpu6000.cpp:34
void attitude_correction_callback(const turbomath::Quaternion &q)
virtual BackupData get_backup_data()=0


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Wed Jul 3 2019 19:59:24