Merged main int branch

avtoku · avtoku · commit 09626b989dfb · 2025-06-19T09:03:02.000-07:00
diff --git a/include/controller.h b/include/controller.h
@@ -65,7 +65,7 @@ class Controller : public ParamListenerInterface
   inline float max_thrust() const { return max_thrust_; }
 
   void init();
-  void run();
+  void run(const float dt);
 
   void calculate_max_thrust();
   void calculate_equilbrium_torque_from_rc();
@@ -78,8 +78,8 @@ class Controller : public ParamListenerInterface
   public:
     PID();
     void init(float kp, float ki, float kd, float max, float min, float tau);
-    float run(float dt, float x, float x_c, bool update_integrator);
-    float run(float dt, float x, float x_c, bool update_integrator, float xdot);
+    float run(const float dt, float x, float x_c, bool update_integrator);
+    float run(const float dt, float x, float x_c, bool update_integrator, float xdot);
 
   private:
     float kp_;
@@ -97,7 +97,7 @@ class Controller : public ParamListenerInterface
 
   ROSflight & RF_;
 
-  Controller::Output run_pid_loops(uint32_t dt, const Estimator::State & state,
+  Controller::Output run_pid_loops(const float dt, const Estimator::State & state,
                                    const control_t & command, bool update_integrators);
 
   Output output_;
diff --git a/include/estimator.h b/include/estimator.h
@@ -69,7 +69,7 @@ class Estimator : public ParamListenerInterface
 
   void init();
   void param_change_callback(uint16_t param_id) override;
-  void run();
+  void run(const float dt);
   void reset_state();
   void reset_adaptive_bias();
   void set_external_attitude_update(const turbomath::Quaternion & q);
@@ -80,7 +80,7 @@ class Estimator : public ParamListenerInterface
   ROSflight & RF_;
   State state_;
 
-  uint64_t last_time_;
+  bool is_initialized_ = false;
   uint64_t last_acc_update_us_;
   uint64_t last_extatt_update_us_;
 
diff --git a/include/rosflight.h b/include/rosflight.h
@@ -68,8 +68,6 @@ class ROSflight
   Sensors sensors_;
   StateManager state_manager_;
 
-  uint32_t loop_time_us;
-
   /**
    * @brief Main initialization routine for the ROSflight autopilot flight stack
    */
@@ -82,10 +80,20 @@ class ROSflight
 
   uint32_t get_loop_time_us();
 
+  /**
+  * @brief Checks to make sure time is going forward. Raises an error if time is detected
+  * to be going backwards.
+  */
+  bool check_time_going_forwards();
+
 private:
   static constexpr size_t num_param_listeners_ = 7;
   ParamListenerInterface * const param_listeners_[num_param_listeners_] = {
     &comm_manager_, &command_manager_, &controller_, &estimator_, &mixer_, &rc_, &sensors_};
+
+  uint32_t loop_time_us_;
+  int64_t last_time_;
+  float dt_;
 };
 
 } // namespace rosflight_firmware
diff --git a/src/controller.cpp b/src/controller.cpp
@@ -107,28 +107,15 @@ bool Controller::is_throttle_high(float threshold) {
           RF_.command_manager_.combined_control().Fz.value > threshold;
 }
 
-void Controller::run()
+void Controller::run(const float dt)
 {
-  // Time calculation
-  if (prev_time_us_ == 0) {
-    prev_time_us_ = RF_.estimator_.state().timestamp_us;
-    return;
-  }
-
-  int32_t dt_us = (RF_.estimator_.state().timestamp_us - prev_time_us_);
-  if (dt_us < 0) {
-    RF_.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS);
-    return;
-  }
-  prev_time_us_ = RF_.estimator_.state().timestamp_us;
-
   // Check if integrators should be updated
   bool update_integrators = (RF_.state_manager_.state().armed)
-    && is_throttle_high(0.1f) && dt_us < 10000;
+    && is_throttle_high(0.1f) && dt < 0.01;
 
   // Run the PID loops
   Controller::Output pid_output = run_pid_loops(
-    dt_us, RF_.estimator_.state(), RF_.command_manager_.combined_control(), update_integrators);
+    dt, RF_.estimator_.state(), RF_.command_manager_.combined_control(), update_integrators);
 
   // Add feedforward torques
   output_.Qx = pid_output.Qx + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE);
@@ -225,14 +212,12 @@ void Controller::param_change_callback(uint16_t param_id)
   }
 }
 
-Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::State & state,
+Controller::Output Controller::run_pid_loops(const float dt, const Estimator::State & state,
                                              const control_t & command, bool update_integrators)
 {
   // Based on the control types coming from the command manager, run the appropriate PID loops
   Controller::Output out;
 
-  float dt = 1e-6 * dt_us;
-
   // ROLL
   if (command.Qx.type == RATE) {
     out.Qx = roll_rate_.run(dt, state.angular_velocity.x, command.Qx.value, update_integrators);
@@ -330,7 +315,7 @@ void Controller::PID::init(float kp, float ki, float kd, float max, float min, f
   tau_ = tau;
 }
 
-float Controller::PID::run(float dt, float x, float x_c, bool update_integrator)
+float Controller::PID::run(const float dt, float x, float x_c, bool update_integrator)
 {
   float xdot;
   if (dt > 0.0001f) {
@@ -348,7 +333,7 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator)
   return run(dt, x, x_c, update_integrator, xdot);
 }
 
-float Controller::PID::run(float dt, float x, float x_c, bool update_integrator, float xdot)
+float Controller::PID::run(const float dt, float x, float x_c, bool update_integrator, float xdot)
 {
   // Calculate Error
   float error = x_c - x;
diff --git a/src/estimator.cpp b/src/estimator.cpp
@@ -91,7 +91,7 @@ void Estimator::reset_adaptive_bias()
 
 void Estimator::init()
 {
-  last_time_ = 0;
+  is_initialized_ = 0;
   last_acc_update_us_ = 0;
   last_extatt_update_us_ = 0;
   reset_state();
@@ -119,31 +119,22 @@ void Estimator::set_external_attitude_update(const turbomath::Quaternion & q)
   q_extatt_ = q;
 }
 
-void Estimator::run()
+void Estimator::run(const float dt)
 {
   //
   // Timing Setup
   //
 
   const uint64_t now_us = RF_.sensors_.get_imu()->header.timestamp;
-  if (last_time_ == 0) {
-    last_time_ = now_us;
+  state_.timestamp_us = now_us;
+
+  if (!is_initialized_) {
     last_acc_update_us_ = now_us;
     last_extatt_update_us_ = now_us;
-    return;
-  } else if (now_us < last_time_) {
-    // this shouldn't happen
-    RF_.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS);
-    last_time_ = now_us;
+    is_initialized_ = true;
     return;
   }
 
-  RF_.state_manager_.clear_error(StateManager::ERROR_TIME_GOING_BACKWARDS);
-
-  float dt = (now_us - last_time_) * 1e-6f;
-  last_time_ = now_us;
-  state_.timestamp_us = now_us;
-
   // Low-pass filter accel and gyro measurements
   run_LPF();
 
diff --git a/src/rosflight.cpp b/src/rosflight.cpp
@@ -46,6 +46,9 @@ ROSflight::ROSflight(Board & board, CommLinkInterface & comm_link)
     , rc_(*this)
     , sensors_(*this)
     , state_manager_(*this)
+    , loop_time_us_(0)
+    , last_time_(0)
+    , dt_(0)
 {
   comm_link.set_listener(&comm_manager_);
   params_.set_listeners(param_listeners_, num_param_listeners_);
@@ -109,13 +112,13 @@ void ROSflight::run()
  
   got_flags got = sensors_.run(); // IMU, GNSS, Baro, Mag, Pitot, SONAR, Battery
 
-  if (got.imu) {
+  if (got.imu && check_time_going_forwards()) { // dt_ is computed by check_time_going_forwards
     uint64_t start = board_.clock_micros();
-    estimator_.run();
-    controller_.run();
+    estimator_.run(dt_);
+    controller_.run(dt_);
     mixer_.mix_output();
     board_.pwm_write(mixer_.raw_outputs(), Mixer::NUM_TOTAL_OUTPUTS);
-    loop_time_us = board_.clock_micros() - start;
+    loop_time_us_ = board_.clock_micros() - start;
   }
 
   /*********************/
@@ -140,6 +143,31 @@ void ROSflight::run()
   command_manager_.run();
 }
 
-uint32_t ROSflight::get_loop_time_us() { return loop_time_us; }
+uint32_t ROSflight::get_loop_time_us() { return loop_time_us_; }
+
+/**
+* @fn bool check_time_going_forwards()
+* @brief Checks to make sure time is going forward. Raises an error if time is detected
+* to be going backwards.
+*/
+bool ROSflight::check_time_going_forwards()
+{
+  const int64_t now_us = static_cast<int64_t>(sensors_.get_imu()->header.timestamp);
+  if (last_time_ == 0) {
+    last_time_ = now_us;
+    return false;
+  }
+  dt_ = (now_us - last_time_) * 1e-6f;
+  last_time_ = now_us;
+
+  // Check if time is going backwards
+  if (dt_ < 0.0) {
+    state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS);
+    return false;
+  }
+
+  state_manager_.clear_error(StateManager::ERROR_TIME_GOING_BACKWARDS);
+  return true;
+}
 
 } // namespace rosflight_firmware
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
@@ -23,6 +23,7 @@ add_executable(unit_tests
         command_manager_test.cpp
         estimator_test.cpp
         parameters_test.cpp
+        rosflight_test.cpp
         )
 
 target_include_directories(unit_tests PUBLIC .)
diff --git a/test/rosflight_test.cpp b/test/rosflight_test.cpp
@@ -0,0 +1,69 @@
+#include "common.h"
+#include "mavlink.h"
+#include "test_board.h"
+#include "state_manager.h"
+
+#include "rosflight.h"
+
+
+using namespace rosflight_firmware;
+
+class ROSflightTest : public ::testing::Test
+{
+public:
+  testBoard board;
+  Mavlink mavlink;
+  ROSflight rf;
+
+  ROSflightTest()
+    : mavlink(board)
+    , rf(board, mavlink)
+  {}
+
+  void SetUp() override
+  {
+    // Initialize firmware with no errors
+    board.backup_memory_clear();
+    rf.init();
+    rf.state_manager_.clear_error(
+      rf.state_manager_.state().error_codes); // Clear All Errors to Start
+    rf.params_.set_param_int(PARAM_PRIMARY_MIXER, 10);
+    rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, false); // default to turning this off
+    rf.params_.set_param_float(PARAM_FAILSAFE_THROTTLE, 0.0f);
+  }
+};
+
+TEST_F(ROSflightTest, MainLoopSetAndClearTimeGoingBackwards)
+{
+  // Set initial time and step
+  board.set_time(100);
+  rf.sensors_.run();
+  board.set_time(500);
+  rf.run();
+
+  // Step time backward
+  board.set_time(100);
+  rf.run();
+
+  EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_TIME_GOING_BACKWARDS);
+
+  // Step time forward
+  board.set_time(500);
+  rf.run();
+  EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE);
+}
+
+TEST_F(ROSflightTest, CheckTimeGoingForward)
+{
+  board.set_time(10000);
+  rf.sensors_.run();
+  EXPECT_EQ(rf.sensors_.get_imu()->header.timestamp, 10000);
+  rf.check_time_going_forwards();
+
+  board.set_time(1000);
+  rf.sensors_.run();
+  rf.check_time_going_forwards();
+
+  EXPECT_EQ(rf.sensors_.get_imu()->header.timestamp, 1000);
+  EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_TIME_GOING_BACKWARDS);
+}

Original file line number	Diff line number	Diff line change
`@@ -23,6 +23,7 @@ add_executable(unit_tests`
`23`	`23`	`command_manager_test.cpp`
`24`	`24`	`estimator_test.cpp`
`25`	`25`	`parameters_test.cpp`
	`26`	`+ rosflight_test.cpp`
`26`	`27`	`)`
`27`	`28`
`28`	`29`	`target_include_directories(unit_tests PUBLIC .)`