bug: fixed initialization error, as well as unsigned data type error. feat: added unit test for rosflight_main loop

Author: JMoore5353

include/rosflight.h

@@ -92,7 +92,7 @@ class ROSflight
     &comm_manager_, &command_manager_, &controller_, &estimator_, &mixer_, &rc_, &sensors_};
 
   uint32_t loop_time_us_;
-  uint64_t last_time_;
+  int64_t last_time_;
   float dt_;
 };
 

src/rosflight.cpp

@@ -46,9 +46,9 @@ ROSflight::ROSflight(Board & board, CommLinkInterface & comm_link)
     , rc_(*this)
     , sensors_(*this)
     , state_manager_(*this)
-    , dt_(0)
-    , last_time_(0)
     , loop_time_us_(0)
+    , last_time_(0)
+    , dt_(0)
 {
   comm_link.set_listener(&comm_manager_);
   params_.set_listeners(param_listeners_, num_param_listeners_);
@@ -112,7 +112,7 @@ void ROSflight::run()
 
   got_flags got = sensors_.run(); // IMU, GNSS, Baro, Mag, Pitot, SONAR, Battery
 
-  if (got.imu && check_time_going_forwards()) {
+  if (got.imu && check_time_going_forwards()) { // dt_ is computed by check_time_going_forwards
     uint64_t start = board_.clock_micros();
     estimator_.run(dt_);
     controller_.run(dt_);
@@ -152,7 +152,7 @@ uint32_t ROSflight::get_loop_time_us() { return loop_time_us_; }
 */
 bool ROSflight::check_time_going_forwards()
 {
-  const uint64_t now_us = sensors_.get_imu()->header.timestamp;
+  const int64_t now_us = static_cast<int64_t>(sensors_.get_imu()->header.timestamp);
   if (last_time_ == 0) {
     last_time_ = now_us;
     return false;

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 .)

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);
+}