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 #include <string.h>
34 
35 #include "rosflight.h"
36 
37 namespace rosflight_firmware
38 {
39 
41 {
42  memset(buffer_, 0, sizeof(buffer_));
43 }
44 
45 
47 {
48  LogMessage& newest_msg = buffer_[newest_];
49  strcpy(newest_msg.msg, msg);
50  newest_msg.severity = severity;
51 
52  newest_ = (newest_ + 1) % LOG_BUF_SIZE;
53 
54  // quietly over-write old messages (what else can we do?)
55  length_ += 1;
56  if (length_ > LOG_BUF_SIZE)
57  {
59  oldest_ = (oldest_ + 1) % LOG_BUF_SIZE;
60  }
61 }
62 
64 {
65  if (length_ > 0)
66  {
67  length_--;
68  oldest_ = (oldest_ + 1) % LOG_BUF_SIZE;
69  }
70 }
71 
72 
74  RF_(rf),
75  comm_link_(comm_link)
76 {}
77 
78 // function definitions
80 {
81  comm_link_.init(static_cast<uint32_t>(RF_.params_.get_param_int(PARAM_BAUD_RATE)),
82  static_cast<uint32_t>(RF_.params_.get_param_int(PARAM_SERIAL_DEVICE)));
83 
86 
100 
101  initialized_ = true;
102 }
103 
104 void CommManager::param_change_callback(uint16_t param_id)
105 {
106  switch (param_id)
107  {
108  case PARAM_SYSTEM_ID:
109  update_system_id(param_id);
110  break;
113  break;
116  break;
119  break;
122  break;
125  break;
128  break;
131  break;
134  break;
137  break;
140  break;
143  break;
146  break;
147  default:
148  // do nothing
149  break;
150  }
151 }
152 
153 void CommManager::update_system_id(uint16_t param_id)
154 {
155  sysid_ = static_cast<uint8_t>(RF_.params_.get_param_int(param_id));
156 }
157 
159 {
160  send_status();
161 }
162 
163 void CommManager::send_param_value(uint16_t param_id)
164 {
165  if (param_id < PARAMS_COUNT)
166  {
167  switch (RF_.params_.get_param_type(param_id))
168  {
169  case PARAM_TYPE_INT32:
171  param_id,
172  RF_.params_.get_param_name(param_id),
173  RF_.params_.get_param_int(param_id),
174  static_cast<uint16_t>(PARAMS_COUNT));
175  break;
176  case PARAM_TYPE_FLOAT:
178  param_id,
179  RF_.params_.get_param_name(param_id),
180  RF_.params_.get_param_float(param_id),
181  static_cast<uint16_t>(PARAMS_COUNT));
182  break;
183  default:
184  break;
185  }
186  }
187 }
188 
189 void CommManager::param_request_list_callback(uint8_t target_system)
190 {
191  if (target_system == sysid_)
192  send_params_index_ = 0;
193 }
194 
196 {
197  send_params_index_ = 0;
198 }
199 
200 void CommManager::param_request_read_callback(uint8_t target_system, const char* const param_name, int16_t param_index)
201 {
202  if (target_system == sysid_)
203  {
204  uint16_t id = (param_index < 0) ? RF_.params_.lookup_param_id(param_name) : static_cast<uint16_t>(param_index);
205 
206  if (id < PARAMS_COUNT)
207  send_param_value(id);
208  }
209 }
210 
211 void CommManager::param_set_int_callback(uint8_t target_system, const char* const param_name, int32_t param_value)
212 {
213  if (target_system == sysid_)
214  {
215  uint16_t id = RF_.params_.lookup_param_id(param_name);
216 
218  {
219  RF_.params_.set_param_int(id, param_value);
220  }
221  }
222 }
223 
224 void CommManager::param_set_float_callback(uint8_t target_system, const char* const param_name, float param_value)
225 {
226  if (target_system == sysid_)
227  {
228  uint16_t id = RF_.params_.lookup_param_id(param_name);
229 
231  {
232  RF_.params_.set_param_float(id, param_value);
233  }
234  }
235 }
236 
238 {
239  bool result;
240  bool reboot_flag = false;
241  bool reboot_to_bootloader_flag = false;
242 
243  // None of these actions can be performed if we are armed
245  {
246  result = false;
247  }
248  else
249  {
250  result = true;
251  switch (command)
252  {
254  result = RF_.params_.read();
255  break;
257  result = RF_.params_.write();
258  break;
261  break;
264  break;
267  break;
270  break;
273  break;
276  break;
278  reboot_flag = true;
279  break;
281  reboot_to_bootloader_flag = true;
282  break;
285  break;
286  }
287  }
288 
289  comm_link_.send_command_ack(sysid_, command, result);
290 
291  if (reboot_flag || reboot_to_bootloader_flag)
292  {
293  RF_.board_.clock_delay(20);
294  RF_.board_.board_reset(reboot_to_bootloader_flag);
295  }
297 }
298 
299 void CommManager::timesync_callback(int64_t tc1, int64_t ts1)
300 {
301  uint64_t now_us = RF_.board_.clock_micros();
302 
303  if (tc1 == 0) // check that this is a request, not a response
304  comm_link_.send_timesync(sysid_, static_cast<int64_t>(now_us)*1000, ts1);
305 }
306 
308 {
309  // put values into a new command struct
310  control_t new_offboard_command;
311  new_offboard_command.x.value = control.x.value;
312  new_offboard_command.y.value = control.y.value;
313  new_offboard_command.z.value = control.z.value;
314  new_offboard_command.F.value = control.F.value;
315 
316  // Move flags into standard message
317  new_offboard_command.x.active = control.x.valid;
318  new_offboard_command.y.active = control.y.valid;
319  new_offboard_command.z.active = control.z.valid;
320  new_offboard_command.F.active = control.F.valid;
321 
322  // translate modes into standard message
323  switch (control.mode)
324  {
326  new_offboard_command.x.type = PASSTHROUGH;
327  new_offboard_command.y.type = PASSTHROUGH;
328  new_offboard_command.z.type = PASSTHROUGH;
329  new_offboard_command.F.type = THROTTLE;
330  break;
332  new_offboard_command.x.type = RATE;
333  new_offboard_command.y.type = RATE;
334  new_offboard_command.z.type = RATE;
335  new_offboard_command.F.type = THROTTLE;
336  break;
338  new_offboard_command.x.type = ANGLE;
339  new_offboard_command.y.type = ANGLE;
340  new_offboard_command.z.type = RATE;
341  new_offboard_command.F.type = THROTTLE;
342  break;
343  }
344 
345  // Tell the command_manager that we have a new command we need to mux
346  new_offboard_command.stamp_ms = RF_.board_.clock_millis();
347  RF_.command_manager_.set_new_offboard_command(new_offboard_command);
348 }
349 
351 {
352  Mixer::aux_command_t new_aux_command;
353 
354  for (int i = 0; i < 14; i++)
355  {
356  switch (command.cmd_array[i].type)
357  {
359  // Channel is either not used or is controlled by the mixer
360  new_aux_command.channel[i].type = Mixer::NONE;
361  new_aux_command.channel[i].value = 0;
362  break;
364  // PWM value should be mapped to servo position
365  new_aux_command.channel[i].type = Mixer::S;
366  new_aux_command.channel[i].value = command.cmd_array[i].value;
367  break;
369  // PWM value should be mapped to motor speed
370  new_aux_command.channel[i].type = Mixer::M;
371  new_aux_command.channel[i].value = command.cmd_array[i].value;
372  break;
373  }
374  }
375 
376  // Send the new aux_command to the mixer
377  RF_.mixer_.set_new_aux_command(new_aux_command);
378 }
379 
381 {
383 }
384 
386 {
387  // receiving a heartbeat implies that a connection has been made
388  // to the off-board computer.
389  connected_ = true;
390 
391  static bool one_time_data_sent = false;
392  if (!one_time_data_sent)
393  {
394  // error data
395  if (this->RF_.board_.has_backup_data())
396  {
397  this->send_error_data();
398  }
399  }
400 
402  // respond to heartbeats with a heartbeat
403  this->send_heartbeat();
404 }
405 
406 // function definitions
408 {
410 }
411 
412 void CommManager::log(CommLinkInterface::LogSeverity severity, const char *fmt, ...)
413 {
414  // Convert the format string to a raw char array
415  va_list args;
416  va_start(args, fmt);
417  char text[LOG_MSG_SIZE];
419  va_end(args);
420 
421  if (initialized_ && connected_)
422  {
423  comm_link_.send_log_message(sysid_, severity, text);
424  }
425  else
426  {
427  log_buffer_.add_message(severity, text);
428  }
429 }
430 
432 {
434 }
435 
437 {
438  if (!initialized_)
439  return;
440 
441  uint8_t control_mode = 0;
443  control_mode = MODE_PASS_THROUGH;
445  control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE;
446  else
448 
455  control_mode,
458 }
459 
460 
462 {
467 }
468 
470 {
471  turbomath::Vector acc, gyro;
472  uint64_t stamp_us;
473  RF_.sensors_.get_filtered_IMU(acc, gyro, stamp_us);
475  stamp_us,
476  acc,
477  gyro,
479 
480 }
481 
483 {
487 }
488 
490 {
491  // TODO better mechanism for retreiving RC (through RC module, not PWM-specific)
492  uint16_t channels[8] = { static_cast<uint16_t>(RF_.board_.rc_read(0)*1000),
493  static_cast<uint16_t>(RF_.board_.rc_read(1)*1000),
494  static_cast<uint16_t>(RF_.board_.rc_read(2)*1000),
495  static_cast<uint16_t>(RF_.board_.rc_read(3)*1000),
496  static_cast<uint16_t>(RF_.board_.rc_read(4)*1000),
497  static_cast<uint16_t>(RF_.board_.rc_read(5)*1000),
498  static_cast<uint16_t>(RF_.board_.rc_read(6)*1000),
499  static_cast<uint16_t>(RF_.board_.rc_read(7)*1000) };
501 }
502 
504 {
506  {
511  }
512 }
513 
515 {
516  if (RF_.sensors_.data().baro_valid)
517  {
522  }
523 }
524 
526 {
528  {
530  0, // TODO set sensor type (sonar/lidar), use enum
532  8.0,
533  0.25);
534  }
535 }
536 
538 {
541 }
542 
544 {
545  BackupData error_data = RF_.board_.get_backup_data();
546  comm_link_.send_error_data(sysid_, error_data);
547 }
548 
550 {
551  const GNSSData& gnss_data = RF_.sensors_.data().gnss_data;
552 
554  {
555  if (gnss_data.time_of_week != last_sent_gnss_tow_)
556  {
557  comm_link_.send_gnss(sysid_, gnss_data);
558  last_sent_gnss_tow_ = gnss_data.time_of_week;
559  }
560  }
561 }
562 
564 {
565  const GNSSRaw& gnss_raw = RF_.sensors_.data().gnss_raw;
566 
568  {
569  if (gnss_raw.time_of_week != last_sent_gnss_raw_tow_)
570  {
573  }
574  }
575 }
576 
578 {
579  send_next_param();
580 
581  // send buffered log messages
582  if (connected_ && !log_buffer_.empty())
583  {
586  log_buffer_.pop();
587  }
588 }
589 
590 // function definitions
592 {
593  uint64_t time_us = RF_.board_.clock_micros();
594  for (int i = 0; i < STREAM_COUNT; i++)
595  {
596  streams_[i].stream(time_us);
597  }
599 }
600 
601 void CommManager::set_streaming_rate(uint8_t stream_id, int16_t param_id)
602 {
603  streams_[stream_id].set_rate(RF_.params_.get_param_int(param_id));
604 }
605 
606 void CommManager::send_named_value_int(const char *const name, int32_t value)
607 {
609 }
610 
611 void CommManager::send_named_value_float(const char *const name, float value)
612 {
614 }
615 
617 {
619  {
620  send_param_value(static_cast<uint16_t>(send_params_index_));
622  }
623 }
624 
625 CommManager::Stream::Stream(uint32_t period_us, std::function<void(void)> send_function) :
626  period_us_(period_us),
627  next_time_us_(0),
628  send_function_(send_function)
629 {}
630 
631 void CommManager::Stream::stream(uint64_t now_us)
632 {
633  if (period_us_ > 0 && now_us >= next_time_us_)
634  {
635  // if you fall behind, skip messages
636  do
637  {
639  }
640  while(next_time_us_ < now_us);
641 
642  send_function_();
643  }
644 }
645 
646 void CommManager::Stream::set_rate(uint32_t rate_hz)
647 {
648  period_us_ = (rate_hz == 0) ? 0 : 1000000/rate_hz;
649 }
650 
651 //void Mavlink::mavlink_send_named_command_struct(const char *const name, control_t command_struct)
652 //{
653 // uint8_t control_mode;
654 // if (command_struct.x.type == RATE && command_struct.y.type == RATE)
655 // {
656 // control_mode = MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE;
657 // }
658 // else if (command_struct.x.type == ANGLE && command_struct.y.type == ANGLE)
659 // {
660 // if (command_struct.x.type == ALTITUDE)
661 // {
662 // control_mode = MODE_ROLL_PITCH_YAWRATE_ALTITUDE;
663 // }
664 // else
665 // {
666 // control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE;
667 // }
668 // }
669 // else
670 // {
671 // control_mode = MODE_PASS_THROUGH;
672 // }
673 // uint8_t ignore = !(command_struct.x.active) ||
674 // !(command_struct.y.active) << 1 ||
675 // !(command_struct.z.active) << 2 ||
676 // !(command_struct.F.active) << 3;
677 // mavlink_message_t msg;
678 // mavlink_msg_named_command_struct_pack(sysid, compid, &msg, name,
679 // control_mode,
680 // ignore,
681 // command_struct.x.value,
682 // command_struct.y.value,
683 // command_struct.z.value,
684 // command_struct.F.value);
685 // send_message(msg);
686 //}
687 
688 } // 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
virtual void send_gnss_raw(uint8_t system_id, const GNSSRaw &data)=0
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:331
const control_t & combined_control() const
const float * get_outputs() const
Definition: mixer.h:269
bool start_imu_calibration(void)
Definition: sensors.cpp:236
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:114
bool read(void)
Read parameter values from non-volatile memory.
Definition: param.cpp:289
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:369
CommManager(ROSflight &rf, CommLinkInterface &comm_link)
void set_new_aux_command(aux_command_t new_aux_command)
Definition: mixer.cpp:157
void send_named_value_int(const char *const name, int32_t value)
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:322
const State & state() const
Definition: estimator.h:64
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:89
virtual void send_gnss(uint8_t system_id, const GNSSData &data)=0
virtual void init(uint32_t baud_rate, uint32_t dev)=0
void set_attitude_correction(const turbomath::Quaternion &q)
Definition: estimator.cpp:120
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_error_data(uint8_t system_id, const BackupData &error_data)=0
virtual void send_heartbeat(uint8_t system_id, bool fixed_wing)=0
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:345
CommLinkInterface & comm_link_
Definition: comm_manager.h:83
bool start_gyro_calibration(void)
Definition: sensors.cpp:247
bool start_baro_calibration(void)
Definition: sensors.cpp:256
#define GIT_VERSION_STRING
Definition: param.cpp:47
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_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:163
turbomath::Quaternion attitude
Definition: estimator.h:55
void get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro, uint64_t &stamp_us)
Definition: sensors.cpp:343
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:332
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:98
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:54
void offboard_control_callback(const CommLinkInterface::OffboardControl &control) override
void attitude_correction_callback(const turbomath::Quaternion &q) override
virtual void send_command_ack(uint8_t system_id, Command command, bool success)=0
int get_param_int(uint16_t id) const
Get the value of an integer parameter by id.
Definition: param.h:312
bool start_diff_pressure_calibration(void)
Definition: sensors.cpp:266
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:259
std::function< void(void)> send_function_
Definition: comm_manager.h:126
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
virtual bool has_backup_data()=0
void param_request_read_callback(uint8_t target_system, const char *const param_name, int16_t param_index) override
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:306
const Data & data() const
Definition: sensors.h:169
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:357
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
virtual BackupData get_backup_data()=0


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:18