added some more dependencies from rosflight_firmware :(

test/interface/comm_link.h

@@ -0,0 +1,255 @@
+/*
+ * 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_LINK_H
+#define ROSFLIGHT_FIRMWARE_COMM_LINK_H
+
+#include "mixer.h"
+#include "param.h"
+#include "sensors.h"
+#include "state_manager.h"
+
+#include <turbomath/turbomath.h>
+
+#include <cstdint>
+
+namespace rosflight_firmware
+{
+
+class CommLinkInterface
+{
+#define cast_in_range(val, type)                                                                   \
+  (static_cast<uint32_t>(val) < static_cast<uint32_t>(type::END)) ? static_cast<type>(val)         \
+                                                                  : type::END
+
+public:
+  enum class LogSeverity
+  {
+    LOG_INFO = 0, // c.f., MAVlink MAV_SEVERITY_INFO=6,
+    LOG_WARNING,  // c.f., MAVlink MAV_SEVERITY_WARNING=4,
+    LOG_ERROR,    // c.f., MAVlink MAV_SEVERITY_ERROR=3,
+    LOG_CRITICAL, // c.f., MAVlink MAV_SEVERITY_CRITICAL=2,
+    END           // c.f., MAVlink
+  };
+
+  enum class CommMessageCommand // c.f., MAVlink ROSFLIGHT_CMD
+  {
+    ROSFLIGHT_CMD_RC_CALIBRATION = 0,         /*  | */
+    ROSFLIGHT_CMD_ACCEL_CALIBRATION = 1,      /*  | */
+    ROSFLIGHT_CMD_GYRO_CALIBRATION = 2,       /*  | */
+    ROSFLIGHT_CMD_BARO_CALIBRATION = 3,       /*  | */
+    ROSFLIGHT_CMD_AIRSPEED_CALIBRATION = 4,   /*  | */
+    ROSFLIGHT_CMD_READ_PARAMS = 5,            /*  | */
+    ROSFLIGHT_CMD_WRITE_PARAMS = 6,           /*  | */
+    ROSFLIGHT_CMD_SET_PARAM_DEFAULTS = 7,     /*  | */
+    ROSFLIGHT_CMD_REBOOT = 8,                 /*  | */
+    ROSFLIGHT_CMD_REBOOT_TO_BOOTLOADER = 9,   /*  | */
+    ROSFLIGHT_CMD_SEND_VERSION = 10,          /*  | */
+    ROSFLIGHT_CMD_RESET_ORIGIN = 11,          /*  | */
+    ROSFLIGHT_CMD_SEND_ALL_CONFIG_INFOS = 12, /*  | */
+    END = 13,                                 /*  | */
+  };
+
+  enum class RosflightAuxCmdType // c.f., MAVlink ROSFLIGHT_AUX_CMD_TYPE
+  {
+    DISABLED = 0, /*  | */
+    SERVO = 1,    /*  | */
+    MOTOR = 2,    /*  | */
+    END = 3
+  };
+
+  enum class RosflightCmdResponse // c.f., MAVlink ROSFLIGHT_CMD_RESPONSE
+  {
+    ROSFLIGHT_CMD_FAILED = 0,  /*  | */
+    ROSFLIGHT_CMD_SUCCESS = 1, /*  | */
+    END = 2
+  };
+
+  enum class OffboardControlMode // c.f., MAVlink OFFBOARD_CONTROL_MODE
+  {
+    MODE_PASS_THROUGH = 0, /* Pass commanded values directly to actuators | */
+    MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE =
+      1, /* Command roll rate, pitch rate, yaw rate, and throttle | */
+    MODE_ROLL_PITCH_YAWRATE_THROTTLE =
+      2, /* Command roll angle, pitch angle, yaw rate, and throttle | */
+    MODE_ROLL_PITCH_YAWRATE_ALTITUDE =
+      3, /* Command roll angle, pitch angle, yaw rate, and altitude above ground | */
+    MODE_XVEL_YVEL_YAWRATE_ALTITUDE =
+      4, /* Command body-fixed, x and y velocity, and yaw rate, and altitude above ground | */
+    MODE_XPOS_YPOS_YAW_ALTITUDE =
+      5, /* Command inertial x, y position (m) wrt origin, yaw angle wrt north, and altitude above ground | */
+    END = 6, /*  | */
+  };
+
+  enum class OffboardControlIgnore // c.f., MAVlink OFFBOARD_CONTROL_IGNORE
+  {
+    IGNORE_NONE = 0x00,   /* Convenience value for specifying no fields should be ignored | */
+    IGNORE_VALUE1 = 0x01, /* Field value1 should be ignored | */
+    IGNORE_VALUE2 = 0x02, /* Field value2 should be ignored | */
+    IGNORE_VALUE3 = 0x04, /* Field value3 should be ignored | */
+    IGNORE_VALUE4 = 0x08, /* Field value4 should be ignored | */
+    IGNORE_VALUE5 = 0x10, /* Field value4 should be ignored | */
+    IGNORE_VALUE6 = 0x20, /* Field value4 should be ignored | */
+    END = 9
+  };
+
+  typedef SensorRangeType RosFlightRangeType;
+
+  enum class CommMessageType
+  {
+    MESSAGE_OFFBOARD_CONTROL = 0,
+    MESSAGE_PARAM_REQUEST_LIST,
+    MESSAGE_PARAM_REQUEST_READ,
+    MESSAGE_PARAM_SET,
+    MESSAGE_ROSFLIGHT_CMD,
+    MESSAGE_ROSFLIGHT_AUX_CMD,
+    MESSAGE_TIMESYNC,
+    MESSAGE_EXTERNAL_ATTITUDE,
+    MESSAGE_HEARTBEAT,
+    END
+  };
+
+  typedef struct
+  {
+    CommMessageType type;
+    union
+    {
+      struct
+      {
+        OffboardControlMode mode;
+        struct Channel
+        {
+          float value;
+          bool valid;
+        };
+        Channel U[6]; // allow for 3 moments and 3 forces.
+      } offboard_control_;
+
+      struct
+      {
+        int16_t
+          id; /*< Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored)*/
+        char name
+          [Params::
+             PARAMS_NAME_LENGTH]; /*< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/
+      } param_read_;
+
+      struct
+      {
+        param_value_t value;
+        char name[Params::PARAMS_NAME_LENGTH];
+        param_type_t type;
+      } param_set_;
+
+      struct
+      {
+        float q[4]; // w,x,y,z
+      } external_attitude_quaternion_;
+
+      struct //
+      {
+        CommMessageCommand command;
+        uint8_t success;
+      } rosflight_cmd_;
+
+      Mixer::aux_command_t new_aux_command_;
+
+      struct
+      {
+        uint64_t local;
+        uint64_t remote;
+      } time_sync_;
+    };
+  } CommMessage;
+
+  virtual void init(uint32_t baud_rate, uint32_t dev) = 0;
+
+  virtual bool parse_char(uint8_t ch, CommMessage * message) = 0;
+
+  virtual void send_attitude_quaternion(uint8_t system_id, const AttitudeStruct & attitude) = 0;
+
+  virtual void send_baro(uint8_t system_id, const PressureStruct & baro) = 0;
+
+  virtual void send_command_ack(uint8_t system_id, uint64_t timestamp_us,
+                                CommMessageCommand command, RosflightCmdResponse success) = 0;
+
+  virtual void send_diff_pressure(uint8_t system_id, const PressureStruct & p) = 0;
+
+  virtual void send_heartbeat(uint8_t system_id, uint64_t timestamp_us, bool fixed_wing) = 0;
+
+  virtual void send_imu(uint8_t system_id, const ImuStruct & imu) = 0;
+
+  virtual void send_log_message(uint8_t system_id, uint64_t timestamp_us, LogSeverity severity,
+                                const char * text) = 0;
+
+  virtual void send_mag(uint8_t system_id, const MagStruct & mag) = 0;
+
+  virtual void send_named_value_int(uint8_t system_id, uint64_t timestamp_us,
+                                    const char * const name, int32_t value) = 0;
+
+  virtual void send_named_value_float(uint8_t system_id, uint64_t timestamp_us,
+                                      const char * const name, float value) = 0;
+
+  virtual void send_output_raw(uint8_t system_id, const RcStruct & rc) = 0;
+
+  virtual void send_param_value_int(uint8_t system_id, uint64_t timestamp_us, uint16_t index,
+                                    const char * const name, int32_t value,
+                                    uint16_t param_count) = 0;
+
+  virtual void send_param_value_float(uint8_t system_id, uint64_t timestamp_us, uint16_t index,
+                                      const char * const name, float value,
+                                      uint16_t param_count) = 0;
+
+  virtual void send_rc_raw(uint8_t system_id, const RcStruct & rc_raw) = 0;
+
+  virtual void send_sonar(uint8_t system_id, const RangeStruct & range) = 0;
+
+  virtual void send_status(uint8_t system_id, uint64_t timestamp_us, 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 void send_timesync(uint8_t system_id, uint64_t timestamp_us, int64_t tc1,
+                             int64_t ts1) = 0;
+
+  virtual void send_version(uint8_t system_id, uint64_t timestamp_us,
+                            const char * const version) = 0;
+
+  virtual void send_gnss(uint8_t system_id, const GnssStruct & gnss) = 0;
+
+  virtual void send_error_data(uint8_t system_id, uint64_t timestamp_us,
+                               const StateManager::BackupData & error_data) = 0;
+
+  virtual void send_battery_status(uint8_t system_id, const BatteryStruct & batt) = 0;
+};
+
+} // namespace rosflight_firmware
+
+#endif // ROSFLIGHT_FIRMWARE_COMM_LINK_H

test/interface/param_listener.h

@@ -0,0 +1,47 @@
+/*
+ * 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_INTERFACE_PARAM_LISTENER_H
+#define ROSFLIGHT_FIRMWARE_INTERFACE_PARAM_LISTENER_H
+
+#include <cstdint>
+
+namespace rosflight_firmware
+{
+class ParamListenerInterface
+{
+public:
+  virtual void param_change_callback(uint16_t param_id) = 0;
+};
+
+} // namespace rosflight_firmware
+
+#endif // ROSFLIGHT_FIRMWARE_INTERFACE_PARAM_LISTENER_H