added even more dependencies from rosflight_firmware :(

test/comm_manager.h

@@ -0,0 +1,169 @@
+/*
+ * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice, this
+ *   list of conditions and the following disclaimer.
+ *
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ *
+ * * Neither the name of the copyright holder nor the names of its
+ *   contributors may be used to endorse or promote products derived from
+ *   this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROSFLIGHT_FIRMWARE_COMM_MANAGER_H
+#define ROSFLIGHT_FIRMWARE_COMM_MANAGER_H
+
+#include "interface/comm_link.h"
+#include "interface/param_listener.h"
+
+#include <stdarg.h>
+#include <stdio.h>
+
+#include <cstdint>
+#include <functional>
+
+namespace rosflight_firmware
+{
+class ROSflight;
+
+class CommManager : public ParamListenerInterface
+{
+private:
+  enum StreamId
+  {
+    STREAM_ID_HEARTBEAT,
+    STREAM_ID_STATUS,
+
+    STREAM_ID_ATTITUDE,
+
+    STREAM_ID_IMU,
+    STREAM_ID_DIFF_PRESSURE,
+    STREAM_ID_BARO,
+    STREAM_ID_SONAR,
+    STREAM_ID_MAG,
+    STREAM_ID_BATTERY_STATUS,
+
+    STREAM_ID_SERVO_OUTPUT_RAW,
+    STREAM_ID_GNSS,
+    STREAM_ID_GNSS_FULL,
+    STREAM_ID_RC_RAW,
+    STREAM_ID_LOW_PRIORITY,
+    STREAM_COUNT
+  };
+
+  enum OffboardControlMode
+  {
+    MODE_PASS_THROUGH,
+    MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE,
+    MODE_ROLL_PITCH_YAWRATE_THROTTLE,
+    MODE_ROLL_PITCH_YAWRATE_ALTITUDE,
+  };
+
+  uint8_t sysid_;
+  uint64_t offboard_control_time_;
+  ROSflight & RF_;
+  CommLinkInterface & comm_link_;
+  uint8_t send_params_index_;
+  bool initialized_ = false;
+  bool connected_ = false;
+
+  static constexpr int LOG_MSG_SIZE = 50;
+  class LogMessageBuffer
+  {
+  public:
+    static constexpr int LOG_BUF_SIZE = 25;
+    LogMessageBuffer();
+
+    struct LogMessage
+    {
+      char msg[LOG_MSG_SIZE];
+      CommLinkInterface::LogSeverity severity;
+    };
+    void add_message(CommLinkInterface::LogSeverity severity, char msg[LOG_MSG_SIZE]);
+    size_t size() const { return length_; }
+    bool empty() const { return length_ == 0; }
+    bool full() const { return length_ == LOG_BUF_SIZE; }
+    const LogMessage & oldest() const { return buffer_[oldest_]; }
+    void pop();
+
+  private:
+    LogMessage buffer_[LOG_BUF_SIZE];
+    size_t oldest_ = 0;
+    size_t newest_ = 0;
+    size_t length_ = 0;
+  };
+  LogMessageBuffer log_buffer_;
+
+  StateManager::BackupData backup_data_buffer_;
+  bool have_backup_data_ = false;
+
+  void update_system_id(uint16_t param_id);
+
+  void send_heartbeat(void);
+  void send_status(void);
+  void send_attitude(void);
+  void send_imu(void);
+  void send_output_raw(void);
+
+  void send_diff_pressure(void);
+  void send_baro(void);
+  void send_sonar(void);
+  void send_mag(void);
+  void send_battery_status(void);
+  void send_gnss(void);
+  void send_gnss_full(void);
+  void send_rosflignt_cmd_ack(void);
+  void send_named_value_int(const char * const name, int32_t value);
+  void send_named_value_float(const char * const name, float value);
+
+  void send_next_param(void);
+
+  void receive_msg_offboard_control(CommLinkInterface::CommMessage * message);
+  void receive_msg_param_request_list(CommLinkInterface::CommMessage * message);
+  void receive_msg_param_request_read(CommLinkInterface::CommMessage * message);
+  void receive_msg_param_set(CommLinkInterface::CommMessage * message);
+  void receive_msg_rosflight_cmd(CommLinkInterface::CommMessage * message);
+  void receive_msg_rosflight_aux_cmd(CommLinkInterface::CommMessage * message);
+  void receive_msg_timesync(CommLinkInterface::CommMessage * message);
+  void receive_msg_external_attitude(CommLinkInterface::CommMessage * message);
+  void receive_msg_heartbeat(CommLinkInterface::CommMessage * message);
+
+public:
+  CommManager(ROSflight & rf, CommLinkInterface & comm_link);
+
+  void init();
+  void param_change_callback(uint16_t param_id) override;
+  void receive(void);
+  void transmit(got_flags got);
+  void send_param_value(uint16_t param_id);
+  void update_status();
+  void log(CommLinkInterface::LogSeverity severity, const char * fmt, ...);
+  void log_message(CommLinkInterface::LogSeverity severity, char * text);
+
+
+  void send_rc_raw(void);
+  void send_backup_data(const StateManager::BackupData & backup_data);
+};
+
+} // namespace rosflight_firmware
+
+#endif // ROSFLIGHT_FIRMWARE_COMM_MANAGER_H

test/command_manager.h

@@ -0,0 +1,170 @@
+/*
+ * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice, this
+ *   list of conditions and the following disclaimer.
+ *
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ *
+ * * Neither the name of the copyright holder nor the names of its
+ *   contributors may be used to endorse or promote products derived from
+ *   this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROSFLIGHT_FIRMWARE_COMMAND_MANAGER_H
+#define ROSFLIGHT_FIRMWARE_COMMAND_MANAGER_H
+
+#include "interface/param_listener.h"
+
+#include "rc.h"
+
+#include <cstdbool>
+#include <cstdint>
+
+namespace rosflight_firmware
+{
+class ROSflight;
+
+typedef enum
+{
+  RATE,        // Channel is is in rate mode (mrad/s)
+  ANGLE,       // Channel command is in angle mode (mrad)
+  THROTTLE,    // Channel is direcly controlling throttle max/1000
+  PASSTHROUGH, // Channel directly passes PWM input to the mixer
+} control_type_t;
+
+typedef struct
+{
+  bool active;         // Whether or not the channel is active
+  control_type_t type; // What type the channel is
+  float value;         // The value of the channel
+} control_channel_t;
+
+typedef struct
+{
+  uint32_t stamp_ms;
+  control_channel_t x;
+  control_channel_t y;
+  control_channel_t z;
+  control_channel_t F;
+} control_t;
+
+class CommandManager : public ParamListenerInterface
+{
+private:
+  typedef struct
+  {
+    control_channel_t * rc;
+    control_channel_t * onboard;
+    control_channel_t * combined;
+  } mux_t;
+
+  mux_t muxes[4] = {{&rc_command_.x, &offboard_command_.x, &combined_command_.x},
+                    {&rc_command_.y, &offboard_command_.y, &combined_command_.y},
+                    {&rc_command_.z, &offboard_command_.z, &combined_command_.z},
+                    {&rc_command_.F, &offboard_command_.F, &combined_command_.F}};
+
+  // clang-format off
+  control_t rc_command_ = {0,
+                           {false, ANGLE, 0.0},
+                           {false, ANGLE, 0.0},
+                           {false, RATE, 0.0},
+                           {false, THROTTLE, 0.0}};
+  control_t offboard_command_ = {0,
+                                 {false, ANGLE, 0.0},
+                                 {false, ANGLE, 0.0},
+                                 {false, RATE, 0.0},
+                                 {false, THROTTLE, 0.0}};
+  control_t combined_command_ = {0,
+                                 {false, ANGLE, 0.0},
+                                 {false, ANGLE, 0.0},
+                                 {false, RATE, 0.0},
+                                 {false, THROTTLE, 0.0}};
+
+  control_t multirotor_failsafe_command_ = {0,
+                                            {true, ANGLE, 0.0},
+                                            {true, ANGLE, 0.0},
+                                            {true, RATE, 0.0},
+                                            {true, THROTTLE, 0.0}};
+  control_t fixedwing_failsafe_command_ = {0,
+                                           {true, PASSTHROUGH, 0.0},
+                                           {true, PASSTHROUGH, 0.0},
+                                           {true, PASSTHROUGH, 0.0},
+                                           {true, THROTTLE, 0.0}};
+  // clang-format on
+
+  typedef enum
+  {
+    ATT_MODE_RATE,
+    ATT_MODE_ANGLE
+  } att_mode_t;
+
+  enum MuxChannel
+  {
+    MUX_X,
+    MUX_Y,
+    MUX_Z,
+    MUX_F,
+  };
+
+  typedef struct
+  {
+    RC::Stick rc_channel;
+    uint32_t last_override_time;
+  } rc_stick_override_t;
+
+  rc_stick_override_t rc_stick_override_[3] = {{RC::STICK_X, 0},
+                                               {RC::STICK_Y, 0},
+                                               {RC::STICK_Z, 0}};
+
+  ROSflight & RF_;
+
+  bool new_command_;
+  bool rc_override_;
+
+  control_t & failsafe_command_;
+
+  void init_failsafe();
+
+  bool do_roll_pitch_yaw_muxing(MuxChannel channel);
+  bool do_throttle_muxing(void);
+  void do_min_throttle_muxing();
+
+  void interpret_rc(void);
+  bool stick_deviated(MuxChannel channel);
+
+public:
+  void param_change_callback(uint16_t param_id) override;
+  CommandManager(ROSflight & _rf);
+  void init();
+  bool run();
+  bool rc_override_active();
+  bool offboard_control_active();
+  void set_new_offboard_command(control_t new_offboard_command);
+  void set_new_rc_command(control_t new_rc_command);
+  void override_combined_command_with_rc();
+  inline const control_t & combined_control() const { return combined_command_; }
+  inline const control_t & rc_control() const { return rc_command_; }
+};
+
+} // namespace rosflight_firmware
+
+#endif // ROSFLIGHT_FIRMWARE_COMMAND_MANAGER_H