Added files

.gitignore

@@ -17,6 +17,7 @@
 /site/
 .idea/
 cmake-build*
+CMakeFiles/
 *.map
 scripts/parameter-descriptions.md
 

CMakeLists.txt

@@ -1,5 +1,13 @@
 ### project settings ###
 
+# reminders:
+# cmake . -DBOARD_TO_BUILD=varmint
+# cmake --build . --target clean -j
+# cmake --build . -j
+# to run test:
+#   cmake . -DBOARD_TO_BUILD=test -DCMAKE_BUILD_TYPE=Release
+#   cmake
+
 cmake_minimum_required(VERSION 3.8)
 project(rosflight_firmware C CXX ASM)
 

comms/mavlink/mavlink.h

@@ -56,58 +56,72 @@ class Mavlink : public CommLinkInterface
   Mavlink(Board & board);
   void init(uint32_t baud_rate, uint32_t dev) override;
 
-  bool parse_char(uint8_t ch, CommMessage *message) override;
-
-  void send_attitude_quaternion(uint8_t system_id, uint64_t timestamp_us,
-                                const turbomath::Quaternion & attitude,
-                                const turbomath::Vector & angular_velocity) override;
-  void send_baro(uint8_t system_id, float altitude, float pressure, float temperature) override;
-  void send_command_ack(uint8_t system_id, CommMessageCommand command, RosflightCmdResponse success) override;
-  void send_diff_pressure(uint8_t system_id, float velocity, float pressure,
-                          float temperature) override;
-  void send_heartbeat(uint8_t system_id, bool fixed_wing) override;
-  void send_imu(uint8_t system_id, uint64_t timestamp_us, const turbomath::Vector & accel,
-                const turbomath::Vector & gyro, float temperature) override;
-  void send_log_message(uint8_t system_id, LogSeverity severity, const char * text) override;
-  void send_mag(uint8_t system_id, const turbomath::Vector & mag) override;
-  void send_named_value_int(uint8_t system_id, uint32_t timestamp_ms, const char * const name,
+  bool parse_char(uint8_t ch, CommMessage * message) override;
+
+  void send_attitude_quaternion(uint8_t system_id, const AttitudeStruct & attitude) override;
+
+  void send_baro(uint8_t system_id, const PressureStruct & baro) override;
+
+  void send_command_ack(uint8_t system_id, uint64_t timestamp_us, CommMessageCommand command,
+                        RosflightCmdResponse success) override;
+
+  void send_diff_pressure(uint8_t system_id, const PressureStruct & p) override;
+
+  void send_heartbeat(uint8_t system_id, uint64_t timestamp_us, bool fixed_wing) override;
+
+  void send_imu(uint8_t system_id, const ImuStruct & imu) override;
+
+  void send_log_message(uint8_t system_id, uint64_t timestamp_us, LogSeverity severity,
+                        const char * text) override;
+
+  void send_mag(uint8_t system_id, const MagStruct & mag) override;
+
+  void send_named_value_int(uint8_t system_id, uint64_t timestamp_us, const char * const name,
                             int32_t value) override;
-  void send_named_value_float(uint8_t system_id, uint32_t timestamp_ms, const char * const name,
+
+  void send_named_value_float(uint8_t system_id, uint64_t imestamp_us, const char * const name,
                               float value) override;
-  void send_output_raw(uint8_t system_id, uint32_t timestamp_ms,
-                       const float raw_outputs[14]) override;
-  void send_param_value_int(uint8_t system_id, uint16_t index, const char * const name,
-                            int32_t value, uint16_t param_count) override;
-  void send_param_value_float(uint8_t system_id, uint16_t index, const char * const name,
-                              float value, uint16_t param_count) override;
-  void send_rc_raw(uint8_t system_id, uint32_t timestamp_ms, const uint16_t channels[8]) override;
-  void send_sonar(uint8_t system_id,
-                  /* TODO enum type*/ uint8_t type, float range, float max_range,
-                  float min_range) override;
-  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) override;
-  void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) override;
-  void send_version(uint8_t system_id, const char * const version) override;
-  void send_gnss(uint8_t system_id, const GNSSData & data) override;
-  void send_gnss_full(uint8_t system_id, const GNSSFull & full) override;
-  void send_error_data(uint8_t system_id, const StateManager::BackupData & error_data) override;
-  void send_battery_status(uint8_t system_id, float voltage, float current) override;
 
-private:
+  void send_output_raw(uint8_t system_id, const RcStruct & raw) override;
 
+  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) override;
+
+  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) override;
+
+  void send_rc_raw(uint8_t system_id, const RcStruct & rc) override;
+
+  void send_sonar(uint8_t system_id, const RangeStruct & sonar) override;
+
+  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) override;
+
+  void send_timesync(uint8_t system_id, uint64_t timestamp_us, int64_t tc1, int64_t ts1) override;
+
+  void send_version(uint8_t system_id, uint64_t timestamp_us, const char * const version) override;
+
+  void send_gnss(uint8_t system_id, const GnssStruct & gnss) override;
+
+  void send_error_data(uint8_t system_id, uint64_t timestamp_us,
+                       const StateManager::BackupData & error_data) override;
+
+  void send_battery_status(uint8_t system_id, const BatteryStruct & batt) override;
+
+private:
   void send_message(const mavlink_message_t & msg, uint8_t qos = UINT8_MAX);
 
-  void handle_msg_param_request_list(const mavlink_message_t * const msg, CommMessage *message);
-  void handle_msg_param_request_read(const mavlink_message_t * const msg, CommMessage *message);
-  void handle_msg_param_set(const mavlink_message_t * const msg, CommMessage *message);
-  void handle_msg_offboard_control(const mavlink_message_t * const msg, CommMessage *message);
-  void handle_msg_external_attitude(const mavlink_message_t * const msg, CommMessage *message);
-  void handle_msg_rosflight_cmd(const mavlink_message_t * const msg, CommMessage *message);
-  void handle_msg_rosflight_aux_cmd(const mavlink_message_t * const msg, CommMessage *message);
-  void handle_msg_timesync(const mavlink_message_t * const msg, CommMessage *message);
-  void handle_msg_heartbeat(const mavlink_message_t * const msg, CommMessage *message);
-  bool handle_mavlink_message(const mavlink_message_t * const msg, CommMessage *message);
+  void handle_msg_param_request_list(const mavlink_message_t * const msg, CommMessage * message);
+  void handle_msg_param_request_read(const mavlink_message_t * const msg, CommMessage * message);
+  void handle_msg_param_set(const mavlink_message_t * const msg, CommMessage * message);
+  void handle_msg_offboard_control(const mavlink_message_t * const msg, CommMessage * message);
+  void handle_msg_external_attitude(const mavlink_message_t * const msg, CommMessage * message);
+  void handle_msg_rosflight_cmd(const mavlink_message_t * const msg, CommMessage * message);
+  void handle_msg_rosflight_aux_cmd(const mavlink_message_t * const msg, CommMessage * message);
+  void handle_msg_timesync(const mavlink_message_t * const msg, CommMessage * message);
+  void handle_msg_heartbeat(const mavlink_message_t * const msg, CommMessage * message);
+  bool handle_mavlink_message(const mavlink_message_t * const msg, CommMessage * message);
 
   Board & board_;
 

include/board.h

@@ -32,13 +32,14 @@
 #ifndef ROSFLIGHT_FIRMWARE_BOARD_H
 #define ROSFLIGHT_FIRMWARE_BOARD_H
 
-#include "sensors.h"
 #include "state_manager.h"
 
 #include <cstdbool>
 #include <cstddef>
 #include <cstdint>
 
+#include <sensors.h>
+
 namespace rosflight_firmware
 {
 class Board
@@ -57,9 +58,9 @@ class Board
   virtual void sensors_init(void) = 0;
   virtual uint16_t sensors_errors_count() = 0;
 
-  virtual uint16_t sensors_init_message_count() =  0;
+  virtual uint16_t sensors_init_message_count() = 0;
   virtual bool sensors_init_message_good(uint16_t i) = 0;
-  virtual uint16_t sensors_init_message(char *message, uint16_t size, uint16_t i) = 0;
+  virtual uint16_t sensors_init_message(char * message, uint16_t size, uint16_t i) = 0;
 
   // clock
   virtual uint32_t clock_millis() = 0;
@@ -75,55 +76,39 @@ class Board
   virtual void serial_flush() = 0;
 
   // IMU
-  virtual bool imu_present() = 0;
-  virtual bool imu_has_new_data() = 0;
-  virtual bool imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time) = 0;
+  virtual bool imu_read(rosflight_firmware::ImuStruct * imu) = 0;
   virtual void imu_not_responding_error() = 0;
 
   // Mag
-  virtual bool mag_present() = 0;
-  virtual bool mag_has_new_data() = 0;
-  virtual bool mag_read(float mag[3]) = 0;
+  virtual bool mag_read(rosflight_firmware::MagStruct * mag) = 0;
 
   // Baro
-  virtual bool baro_present() = 0;
-  virtual bool baro_has_new_data() = 0;
-  virtual bool baro_read(float * pressure, float * temperature) = 0;
+  virtual bool baro_read(PressureStruct * diff_pressure) = 0;
 
   // Pitot
-  virtual bool diff_pressure_present() = 0;
-  virtual bool diff_pressure_has_new_data() = 0;
-  virtual bool diff_pressure_read(float * diff_pressure, float * temperature) = 0;
+  virtual bool diff_pressure_read(PressureStruct * diff_pressure) = 0;
 
   // Sonar
-  virtual bool sonar_present() = 0;
-  virtual bool sonar_has_new_data() = 0;
-  virtual bool sonar_read(float * range) = 0;
+  virtual bool sonar_read(RangeStruct * sonar) = 0;
 
   // GPS
-  virtual bool gnss_present() = 0;
-  virtual bool gnss_has_new_data() = 0;
-  virtual bool gnss_read(GNSSData * gnss, GNSSFull * gnss_full) = 0;
-
+  virtual bool gnss_read(rosflight_firmware::GnssStruct * gnss) = 0;
   // Battery
-  virtual bool battery_present() = 0;
-  virtual bool battery_has_new_data() = 0;
-  virtual bool battery_read(float * voltage, float * current) = 0;
+  virtual bool battery_read(rosflight_firmware::BatteryStruct * bat) = 0;
   virtual void battery_voltage_set_multiplier(double multiplier) = 0;
   virtual void battery_current_set_multiplier(double multiplier) = 0;
 
   // RC
   virtual void rc_init(rc_type_t rc_type) = 0;
   virtual bool rc_lost() = 0;
-  virtual bool rc_has_new_data() = 0;
-  virtual float rc_read(uint8_t chan) = 0;
+  //virtual bool rc_has_new_data() = 0;
+  //virtual float rc_read(uint8_t chan) = 0;
+  virtual bool rc_read(rosflight_firmware::RcStruct * rc) = 0;
 
   // PWM
-  virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0;
-  virtual void pwm_init_multi(const float * rate, uint32_t channels) = 0;
+  virtual void pwm_init(const float * rate, uint32_t channels) = 0;
   virtual void pwm_disable() = 0;
-  virtual void pwm_write(uint8_t channel, float value) = 0;
-  virtual void pwm_write_multi(float * value, uint32_t channels) = 0;
+  virtual void pwm_write(float * value, uint32_t channels) = 0;
 
   // non-volatile memory
   virtual void memory_init() = 0;

include/comm_manager.h

@@ -45,7 +45,7 @@ namespace rosflight_firmware
 {
 class ROSflight;
 
-class CommManager :  public ParamListenerInterface
+class CommManager : public ParamListenerInterface
 {
 private:
   enum StreamId
@@ -116,37 +116,8 @@ class CommManager :  public ParamListenerInterface
   StateManager::BackupData backup_data_buffer_;
   bool have_backup_data_ = false;
 
-  class Stream
-  {
-  public:
-    Stream(uint32_t period_us, std::function<void(void)> send_function);
-
-    void stream(uint64_t now_us);
-    void set_rate(uint32_t rate_hz);
-
-    uint32_t period_us_;
-    uint64_t next_time_us_;
-    std::function<void(void)> send_function_;
-  };
-
   void update_system_id(uint16_t param_id);
 
-  // The following are CommLinkInterface Listener Functions
-  //
-  //void param_request_list_callback(uint8_t target_system) override;
-//  void param_request_read_callback(uint8_t target_system, const char * const param_name,
-//                                   int16_t param_index) override;
-//  void param_set_int_callback(uint8_t target_system, const char * const param_name,
-//                              int32_t param_value) override;
-//  void param_set_float_callback(uint8_t target_system, const char * const param_name,
-//                                float param_value) override;
-//  void command_callback(CommLinkInterface::Command command) override;
-  //void timesync_callback(int64_t tc1, int64_t ts1) override;
-  //void offboard_control_callback(const CommLinkInterface::OffboardControl & control) override;
-  //void aux_command_callback(const CommLinkInterface::AuxCommand & command) override;
-  //void external_attitude_callback(const turbomath::Quaternion & q) override;
-  //void heartbeat_callback() override;
-
   void send_heartbeat(void);
   void send_status(void);
   void send_attitude(void);
@@ -160,39 +131,34 @@ class CommManager :  public ParamListenerInterface
   void send_battery_status(void);
   void send_gnss(void);
   void send_gnss_full(void);
-  void send_low_priority(void);
   void send_rosflignt_cmd_ack(void);
   void send_named_value_int(const char * const name, int32_t value);
-  void send_next_param(void);
+  void send_named_value_float(const char * const name, float value);
 
-  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);
+  void send_next_param(void);
 
-  // the time of week stamp for the last sent GNSS message, to prevent re-sending
-  uint32_t last_sent_gnss_tow_ = 0;
-  uint32_t last_sent_gnss_full_tow_ = 0;
+  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 stream(got_flags got);
+  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_named_value_float(const char * const name, float value);
-
   void send_backup_data(const StateManager::BackupData & backup_data);
 };
 

include/command_manager.h

@@ -142,7 +142,6 @@ class CommandManager : public ParamListenerInterface
 
   control_t & failsafe_command_;
 
-  void param_change_callback(uint16_t param_id) override;
   void init_failsafe();
 
   bool do_roll_pitch_yaw_muxing(MuxChannel channel);
@@ -153,6 +152,7 @@ class CommandManager : public ParamListenerInterface
   bool stick_deviated(MuxChannel channel);
 
 public:
+  void param_change_callback(uint16_t param_id) override;
   CommandManager(ROSflight & _rf);
   void init();
   bool run();

include/estimator.h

@@ -46,7 +46,15 @@ class ROSflight;
 
 class Estimator : public ParamListenerInterface
 {
+
 public:
+  typedef struct //__attribute__((__packed__))
+  {
+    uint64_t timestamp; // us, time of data read complete
+    float q[4];         // quaternions
+    float rate[3];
+  } AttitudeStruct;
+
   struct State
   {
     turbomath::Vector angular_velocity;
@@ -74,7 +82,10 @@ class Estimator : public ParamListenerInterface
   void reset_adaptive_bias();
   void set_external_attitude_update(const turbomath::Quaternion & q);
 
+  AttitudeStruct * get_attitude(void) { return &attitude_; }
+
 private:
+  AttitudeStruct attitude_;
   const turbomath::Vector g_ = {0.0f, 0.0f, -1.0f};
 
   ROSflight & RF_;