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 "comm_manager.h"
33 
34 #include "param.h"
35 
36 #include "rosflight.h"
37 
38 #include <cstdint>
39 #include <cstring>
40 
41 namespace rosflight_firmware
42 {
44 {
45  memset(buffer_, 0, sizeof(buffer_));
46 }
47 
49 {
50  LogMessage& newest_msg = buffer_[newest_];
51  strcpy(newest_msg.msg, msg);
52  newest_msg.severity = severity;
53 
54  newest_ = (newest_ + 1) % LOG_BUF_SIZE;
55 
56  // quietly over-write old messages (what else can we do?)
57  length_ += 1;
58  if (length_ > LOG_BUF_SIZE)
59  {
61  oldest_ = (oldest_ + 1) % LOG_BUF_SIZE;
62  }
63 }
64 
66 {
67  if (length_ > 0)
68  {
69  length_--;
70  oldest_ = (oldest_ + 1) % LOG_BUF_SIZE;
71  }
72 }
73 
74 CommManager::CommManager(ROSflight& rf, CommLinkInterface& comm_link) : RF_(rf), comm_link_(comm_link) {}
75 
76 // function definitions
78 {
79  comm_link_.init(static_cast<uint32_t>(RF_.params_.get_param_int(PARAM_BAUD_RATE)),
80  static_cast<uint32_t>(RF_.params_.get_param_int(PARAM_SERIAL_DEVICE)));
81 
84 
99 
100  initialized_ = true;
101 }
102 
103 void CommManager::param_change_callback(uint16_t param_id)
104 {
105  switch (param_id)
106  {
107  case PARAM_SYSTEM_ID:
108  update_system_id(param_id);
109  break;
112  break;
115  break;
118  break;
121  break;
124  break;
127  break;
130  break;
133  break;
136  break;
139  break;
142  break;
145  break;
148  break;
149  default:
150  // do nothing
151  break;
152  }
153 }
154 
155 void CommManager::update_system_id(uint16_t param_id)
156 {
157  sysid_ = static_cast<uint8_t>(RF_.params_.get_param_int(param_id));
158 }
159 
161 {
162  send_status();
163 }
164 
165 void CommManager::send_param_value(uint16_t param_id)
166 {
167  if (param_id < PARAMS_COUNT)
168  {
169  switch (RF_.params_.get_param_type(param_id))
170  {
171  case PARAM_TYPE_INT32:
173  RF_.params_.get_param_int(param_id), static_cast<uint16_t>(PARAMS_COUNT));
174  break;
175  case PARAM_TYPE_FLOAT:
177  RF_.params_.get_param_float(param_id), static_cast<uint16_t>(PARAMS_COUNT));
178  break;
179  default:
180  break;
181  }
182  }
183 }
184 
185 void CommManager::param_request_list_callback(uint8_t target_system)
186 {
187  if (target_system == sysid_)
188  send_params_index_ = 0;
189 }
190 
192 {
193  send_params_index_ = 0;
194 }
195 
196 void CommManager::param_request_read_callback(uint8_t target_system, const char* const param_name, int16_t param_index)
197 {
198  if (target_system == sysid_)
199  {
200  uint16_t id = (param_index < 0) ? RF_.params_.lookup_param_id(param_name) : static_cast<uint16_t>(param_index);
201 
202  if (id < PARAMS_COUNT)
203  send_param_value(id);
204  }
205 }
206 
207 void CommManager::param_set_int_callback(uint8_t target_system, const char* const param_name, int32_t param_value)
208 {
209  if (target_system == sysid_)
210  {
211  uint16_t id = RF_.params_.lookup_param_id(param_name);
212 
214  {
215  RF_.params_.set_param_int(id, param_value);
216  }
217  }
218 }
219 
220 void CommManager::param_set_float_callback(uint8_t target_system, const char* const param_name, float param_value)
221 {
222  if (target_system == sysid_)
223  {
224  uint16_t id = RF_.params_.lookup_param_id(param_name);
225 
227  {
228  RF_.params_.set_param_float(id, param_value);
229  }
230  }
231 }
232 
234 {
235  bool result;
236  bool reboot_flag = false;
237  bool reboot_to_bootloader_flag = false;
238 
239  // None of these actions can be performed if we are armed
241  {
242  result = false;
243  }
244  else
245  {
246  result = true;
247  switch (command)
248  {
250  result = RF_.params_.read();
251  break;
253  result = RF_.params_.write();
254  break;
257  break;
260  break;
263  break;
266  break;
269  break;
272  break;
274  reboot_flag = true;
275  break;
277  reboot_to_bootloader_flag = true;
278  break;
281  break;
282  }
283  }
284 
285  comm_link_.send_command_ack(sysid_, command, result);
286 
287  if (reboot_flag || reboot_to_bootloader_flag)
288  {
289  RF_.board_.clock_delay(20);
290  RF_.board_.board_reset(reboot_to_bootloader_flag);
291  }
293 }
294 
295 void CommManager::timesync_callback(int64_t tc1, int64_t ts1)
296 {
297  uint64_t now_us = RF_.board_.clock_micros();
298 
299  if (tc1 == 0) // check that this is a request, not a response
300  comm_link_.send_timesync(sysid_, static_cast<int64_t>(now_us) * 1000, ts1);
301 }
302 
304 {
305  // put values into a new command struct
306  control_t new_offboard_command;
307  new_offboard_command.x.value = control.x.value;
308  new_offboard_command.y.value = control.y.value;
309  new_offboard_command.z.value = control.z.value;
310  new_offboard_command.F.value = control.F.value;
311 
312  // Move flags into standard message
313  new_offboard_command.x.active = control.x.valid;
314  new_offboard_command.y.active = control.y.valid;
315  new_offboard_command.z.active = control.z.valid;
316  new_offboard_command.F.active = control.F.valid;
317 
318  // translate modes into standard message
319  switch (control.mode)
320  {
322  new_offboard_command.x.type = PASSTHROUGH;
323  new_offboard_command.y.type = PASSTHROUGH;
324  new_offboard_command.z.type = PASSTHROUGH;
325  new_offboard_command.F.type = THROTTLE;
326  break;
328  new_offboard_command.x.type = RATE;
329  new_offboard_command.y.type = RATE;
330  new_offboard_command.z.type = RATE;
331  new_offboard_command.F.type = THROTTLE;
332  break;
334  new_offboard_command.x.type = ANGLE;
335  new_offboard_command.y.type = ANGLE;
336  new_offboard_command.z.type = RATE;
337  new_offboard_command.F.type = THROTTLE;
338  break;
339  }
340 
341  // Tell the command_manager that we have a new command we need to mux
342  new_offboard_command.stamp_ms = RF_.board_.clock_millis();
343  RF_.command_manager_.set_new_offboard_command(new_offboard_command);
344 }
345 
347 {
348  Mixer::aux_command_t new_aux_command;
349 
350  for (int i = 0; i < 14; i++)
351  {
352  switch (command.cmd_array[i].type)
353  {
355  // Channel is either not used or is controlled by the mixer
356  new_aux_command.channel[i].type = Mixer::NONE;
357  new_aux_command.channel[i].value = 0;
358  break;
360  // PWM value should be mapped to servo position
361  new_aux_command.channel[i].type = Mixer::S;
362  new_aux_command.channel[i].value = command.cmd_array[i].value;
363  break;
365  // PWM value should be mapped to motor speed
366  new_aux_command.channel[i].type = Mixer::M;
367  new_aux_command.channel[i].value = command.cmd_array[i].value;
368  break;
369  }
370  }
371 
372  // Send the new aux_command to the mixer
373  RF_.mixer_.set_new_aux_command(new_aux_command);
374 }
375 
377 {
379 }
380 
382 {
383  // receiving a heartbeat implies that a connection has been made
384  // to the off-board computer.
385  connected_ = true;
386 
387  // send backup data if we have it buffered
388  if (have_backup_data_)
389  {
391  have_backup_data_ = false;
392  }
393 
395  // respond to heartbeats with a heartbeat
396  this->send_heartbeat();
397 }
398 
399 // function definitions
401 {
403 }
404 
405 void CommManager::log(CommLinkInterface::LogSeverity severity, const char* fmt, ...)
406 {
407  // Convert the format string to a raw char array
408  va_list args;
409  va_start(args, fmt);
410  char text[LOG_MSG_SIZE];
412  va_end(args);
413 
414  if (initialized_ && connected_)
415  {
416  comm_link_.send_log_message(sysid_, severity, text);
417  }
418  else
419  {
420  log_buffer_.add_message(severity, text);
421  }
422 }
423 
425 {
427 }
428 
430 {
431  if (!initialized_)
432  return;
433 
434  uint8_t control_mode = 0;
436  control_mode = MODE_PASS_THROUGH;
438  control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE;
439  else
441 
446 }
447 
449 {
452 }
453 
455 {
456  turbomath::Vector acc, gyro;
457  uint64_t stamp_us;
458  RF_.sensors_.get_filtered_IMU(acc, gyro, stamp_us);
459  comm_link_.send_imu(sysid_, stamp_us, acc, gyro, RF_.sensors_.data().imu_temperature);
460 }
461 
463 {
465 }
466 
468 {
469  // TODO better mechanism for retreiving RC (through RC module, not PWM-specific)
470  uint16_t channels[8] = {static_cast<uint16_t>(RF_.board_.rc_read(0) * 1000 + 1000),
471  static_cast<uint16_t>(RF_.board_.rc_read(1) * 1000 + 1000),
472  static_cast<uint16_t>(RF_.board_.rc_read(2) * 1000 + 1000),
473  static_cast<uint16_t>(RF_.board_.rc_read(3) * 1000 + 1000),
474  static_cast<uint16_t>(RF_.board_.rc_read(4) * 1000 + 1000),
475  static_cast<uint16_t>(RF_.board_.rc_read(5) * 1000 + 1000),
476  static_cast<uint16_t>(RF_.board_.rc_read(6) * 1000 + 1000),
477  static_cast<uint16_t>(RF_.board_.rc_read(7) * 1000 + 1000)};
479 }
480 
482 {
484  {
487  }
488 }
489 
491 {
492  if (RF_.sensors_.data().baro_valid)
493  {
496  }
497 }
498 
500 {
502  {
504  0, // TODO set sensor type (sonar/lidar), use enum
505  RF_.sensors_.data().sonar_range, 8.0, 0.25);
506  }
507 }
508 
510 {
513 }
515 {
518 }
519 
520 void CommManager::send_backup_data(const StateManager::BackupData& backup_data)
521 {
522  if (connected_)
523  {
524  comm_link_.send_error_data(sysid_, backup_data);
525  }
526  else
527  {
528  backup_data_buffer_ = backup_data;
529  have_backup_data_ = true;
530  }
531 }
532 
534 {
535  const GNSSData& gnss_data = RF_.sensors_.data().gnss_data;
536 
538  {
539  if (gnss_data.time_of_week != last_sent_gnss_tow_)
540  {
541  comm_link_.send_gnss(sysid_, gnss_data);
542  last_sent_gnss_tow_ = gnss_data.time_of_week;
543  }
544  }
545 }
546 
548 {
549  const GNSSFull& gnss_full = RF_.sensors_.data().gnss_full;
550 
552  {
553  if (gnss_full.time_of_week != last_sent_gnss_full_tow_)
554  {
557  }
558  }
559 }
560 
562 {
563  send_next_param();
564 
565  // send buffered log messages
566  if (connected_ && !log_buffer_.empty())
567  {
570  log_buffer_.pop();
571  }
572 }
573 
574 // function definitions
576 {
577  uint64_t time_us = RF_.board_.clock_micros();
578  for (int i = 0; i < STREAM_COUNT; i++)
579  {
580  streams_[i].stream(time_us);
581  }
583 }
584 
585 void CommManager::set_streaming_rate(uint8_t stream_id, int16_t param_id)
586 {
587  streams_[stream_id].set_rate(RF_.params_.get_param_int(param_id));
588 }
589 
590 void CommManager::send_named_value_int(const char* const name, int32_t value)
591 {
593 }
594 
595 void CommManager::send_named_value_float(const char* const name, float value)
596 {
598 }
599 
601 {
603  {
604  send_param_value(static_cast<uint16_t>(send_params_index_));
606  }
607 }
608 
609 CommManager::Stream::Stream(uint32_t period_us, std::function<void(void)> send_function) :
610  period_us_(period_us),
611  next_time_us_(0),
612  send_function_(send_function)
613 {
614 }
615 
616 void CommManager::Stream::stream(uint64_t now_us)
617 {
618  if (period_us_ > 0 && now_us >= next_time_us_)
619  {
620  // if you fall behind, skip messages
621  do
622  {
624  } while (next_time_us_ < now_us);
625 
626  send_function_();
627  }
628 }
629 
630 void CommManager::Stream::set_rate(uint32_t rate_hz)
631 {
632  period_us_ = (rate_hz == 0) ? 0 : 1000000 / rate_hz;
633 }
634 
635 // void Mavlink::mavlink_send_named_command_struct(const char *const name, control_t command_struct)
636 //{
637 // uint8_t control_mode;
638 // if (command_struct.x.type == RATE && command_struct.y.type == RATE)
639 // {
640 // control_mode = MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE;
641 // }
642 // else if (command_struct.x.type == ANGLE && command_struct.y.type == ANGLE)
643 // {
644 // if (command_struct.x.type == ALTITUDE)
645 // {
646 // control_mode = MODE_ROLL_PITCH_YAWRATE_ALTITUDE;
647 // }
648 // else
649 // {
650 // control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE;
651 // }
652 // }
653 // else
654 // {
655 // control_mode = MODE_PASS_THROUGH;
656 // }
657 // uint8_t ignore = !(command_struct.x.active) ||
658 // !(command_struct.y.active) << 1 ||
659 // !(command_struct.z.active) << 2 ||
660 // !(command_struct.F.active) << 3;
661 // mavlink_message_t msg;
662 // mavlink_msg_named_command_struct_pack(sysid, compid, &msg, name,
663 // control_mode,
664 // ignore,
665 // command_struct.x.value,
666 // command_struct.y.value,
667 // command_struct.z.value,
668 // command_struct.F.value);
669 // send_message(msg);
670 //}
671 
672 } // namespace rosflight_firmware
virtual void send_diff_pressure(uint8_t system_id, float velocity, float pressure, float temperature)=0
void set_streaming_rate(uint8_t stream_id, int16_t param_id)
virtual float rc_read(uint8_t channel)=0
static volatile int16_t gyro[3]
Definition: drv_mpu6050.c:277
void param_set_float_callback(uint8_t target_system, const char *const param_name, float param_value) override
void log(CommLinkInterface::LogSeverity severity, const char *fmt,...)
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:340
const control_t & combined_control() const
const float * get_outputs() const
Definition: mixer.h:228
bool start_imu_calibration(void)
Definition: sensors.cpp:253
virtual void send_rc_raw(uint8_t system_id, uint32_t timestamp_ms, const uint16_t channels[8])=0
void timesync_callback(int64_t tc1, int64_t ts1) override
virtual void send_attitude_quaternion(uint8_t system_id, uint64_t timestamp_us, const turbomath::Quaternion &attitude, const turbomath::Vector &angular_velocity)=0
void set_defaults(void)
Set all parameters to default values.
Definition: param.cpp:112
bool read(void)
Read parameter values from non-volatile memory.
Definition: param.cpp:298
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:378
CommManager(ROSflight &rf, CommLinkInterface &comm_link)
void set_new_aux_command(aux_command_t new_aux_command)
Definition: mixer.cpp:151
void send_named_value_int(const char *const name, int32_t value)
StateManager::BackupData backup_data_buffer_
Definition: comm_manager.h:115
virtual void send_battery_status(uint8_t system_id, float voltage, float current)=0
void send_param_value(uint16_t param_id)
virtual void send_baro(uint8_t system_id, float altitude, float pressure, float temperature)=0
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
virtual void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1)=0
static constexpr int LOG_MSG_SIZE
Definition: comm_manager.h:88
virtual void send_gnss(uint8_t system_id, const GNSSData &data)=0
virtual void init(uint32_t baud_rate, uint32_t dev)=0
virtual void serial_flush()=0
Stream(uint32_t period_us, std::function< void(void)> send_function)
virtual void send_param_value_float(uint8_t system_id, uint16_t index, const char *const name, float value, uint16_t param_count)=0
virtual void send_heartbeat(uint8_t system_id, bool fixed_wing)=0
void set_external_attitude_update(const turbomath::Quaternion &q)
Definition: estimator.cpp:119
virtual void board_reset(bool bootloader)=0
virtual void send_log_message(uint8_t system_id, LogSeverity severity, const char *text)=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:343
CommLinkInterface & comm_link_
Definition: comm_manager.h:83
virtual void send_error_data(uint8_t system_id, const StateManager::BackupData &error_data)=0
bool start_gyro_calibration(void)
Definition: sensors.cpp:264
bool start_baro_calibration(void)
Definition: sensors.cpp:273
#define GIT_VERSION_STRING
Definition: param.cpp:47
virtual uint64_t clock_micros()=0
const State & state() const
void set_new_offboard_command(control_t new_offboard_command)
void param_change_callback(uint16_t param_id) override
virtual uint16_t num_sensor_errors()=0
void aux_command_callback(const CommLinkInterface::AuxCommand &command) override
virtual void send_version(uint8_t system_id, const char *const version)=0
virtual void clock_delay(uint32_t milliseconds)=0
Stream streams_[STREAM_COUNT]
Definition: comm_manager.h:165
turbomath::Quaternion attitude
Definition: estimator.h:53
void get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro, uint64_t &stamp_us)
Definition: sensors.cpp:359
virtual void send_imu(uint8_t system_id, uint64_t timestamp_us, const turbomath::Vector &accel, const turbomath::Vector &gyro, float temperature)=0
const char * get_param_name(uint16_t id) const
Get the name of a parameter.
Definition: param.h:333
virtual void send_named_value_int(uint8_t system_id, uint32_t timestamp_ms, const char *const name, int32_t value)=0
aux_channel_t channel[NUM_TOTAL_OUTPUTS]
Definition: mixer.h:94
void send_backup_data(const StateManager::BackupData &backup_data)
void param_set_int_callback(uint8_t target_system, const char *const param_name, int32_t param_value) override
virtual uint32_t clock_millis()=0
void param_request_list_callback(uint8_t target_system) override
turbomath::Vector angular_velocity
Definition: estimator.h:52
void offboard_control_callback(const CommLinkInterface::OffboardControl &control) override
virtual void send_command_ack(uint8_t system_id, Command command, bool success)=0
void external_attitude_callback(const turbomath::Quaternion &q) override
int get_param_int(uint16_t id) const
Get the value of an integer parameter by id.
Definition: param.h:319
bool start_diff_pressure_calibration(void)
Definition: sensors.cpp:283
virtual void send_output_raw(uint8_t system_id, uint32_t timestamp_ms, const float raw_outputs[14])=0
virtual void send_named_value_float(uint8_t system_id, uint32_t timestamp_ms, const char *const name, float value)=0
void command_callback(CommLinkInterface::Command command) override
void tfp_sprintf(char *s, const char *fmt, va_list va)
Definition: nanoprintf.cpp:252
std::function< void(void)> send_function_
Definition: comm_manager.h:128
virtual void send_status(uint8_t system_id, bool armed, bool failsafe, bool rc_override, bool offboard, uint8_t error_code, uint8_t control_mode, int16_t num_errors, int16_t loop_time_us)=0
void param_request_read_callback(uint8_t target_system, const char *const param_name, int16_t param_index) override
virtual void send_gnss_full(uint8_t system_id, const GNSSFull &data)=0
void send_named_value_float(const char *const name, float value)
virtual void send_param_value_int(uint8_t system_id, uint16_t index, const char *const name, int32_t value, uint16_t param_count)=0
void add_message(CommLinkInterface::LogSeverity severity, char msg[LOG_MSG_SIZE])
bool write(void)
Write current parameter values to non-volatile memory.
Definition: param.cpp:315
const Data & data() const
Definition: sensors.h:167
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:366
virtual void send_mag(uint8_t system_id, const turbomath::Vector &mag)=0
virtual void send_sonar(uint8_t system_id, uint8_t type, float range, float max_range, float min_range)=0


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Sat May 9 2020 03:16:52