Fixed state_.timestamp bug in estimator

[JMoore5353]

This PR fixes a bug with the initialization of the timestamp_us field in the firmware estimator. This error was causing the ERROR_TIME_GOING_BACKWARDS error to trigger nonstop on boot.

Previously on startup (the first time through the estimator loop), the estimator initializes some variables and returns. The controller then queries some of those variables the first time through its loop. However, the state_.timestamp_us field was not initialized, so the controller was reading in random values for that variable.

I also updated the prev_time_us_ variable in the controller. Otherwise, if the ERROR_TIME_GOING_BACKWARDS does get triggered, then the controller has no way to not trigger on the next iteration.

[avtoku]

This is a Band-Aid we should fix.
Doing the whole dt and time going backward logic in both Controller::run() and Estimator::run() is messy. I suggest moving that up into the if(got.imu) {...} in void ROSflight::run() with dt passed to Controller::run() and Estimater::run()For the case of time going backward (you are in big trouble in this case), you just do not execute what is inside the if statement (or better yet trigger a failsafe mode?).

[JMoore5353]

This is a Band-Aid we should fix. Doing the whole dt and time going backward logic in both Controller::run() and Estimator::run() is messy. I suggest moving that up into the if(got.imu) {...} in void ROSflight::run() with dt passed to Controller::run() and Estimater::run()For the case of time going backward (you are in big trouble in this case), you just do not execute what is inside the if statement (or better yet trigger a failsafe mode?).

Good suggestion. I moved the check to the main loop. I didn't add support for a failsafe, but it should be straightforward to do.

[Copilot]

Pull Request Overview

This PR addresses an uninitialized timestamp in the estimator by introducing a time‐forward check in ROSflight, modifying the estimator to set its timestamp on first run, and updating the controller to use a passed‐in dt instead of raw microseconds.

• Enforce monotonically increasing time before calling estimator and controller
• Change Estimator::run() and Controller::run() to accept a dt parameter and remove their internal time‐tracking
• Track loop execution time in loop_time_us_ and replace the old controller prev_time_us_ logic

Reviewed Changes

Copilot reviewed 6 out of 6 changed files in this pull request and generated 2 comments.

Show a summary per file

File	Description
src/rosflight.cpp	Added dt_, last_time_, loop_time_us_; introduced check_time_going_forwards() and updated run loop call
include/rosflight.h	Declared new members dt_, last_time_, loop_time_us_ and check_time_going_forwards()
src/estimator.cpp	Initialize state_.timestamp_us on first run, replaced last_time_ logic with is_initialized_ flag
include/estimator.h	Updated run() signature to run(const float dt) and replaced last_time_ with is_initialized_
src/controller.cpp	Updated run() and run_pid_loops() signatures to use const float dt and removed old prev_time_us_ logic
include/controller.h	Updated run() and run_pid_loops() signatures to include const float dt

Comments suppressed due to low confidence (3)

include/rosflight.h:94

• [nitpick] Consider providing default initializers for loop_time_us_, last_time_, and dt_ in the class definition (e.g., uint32_t loop_time_us_ = 0; uint64_t last_time_ = 0; float dt_ = 0.0f;) to document their intended initial state and avoid relying solely on constructor initialization.

  uint32_t loop_time_us_;

src/rosflight.cpp:148

• [nitpick] Expand the doc comment for check_time_going_forwards() to explain its initial-iteration behavior (when last_time_ == 0) and how it affects skipping the first loop iteration.

* @brief Checks to make sure time is going forward. Raises an error if time is detected

src/rosflight.cpp:113

• [nitpick] Add unit tests for check_time_going_forwards() to cover the initial timestamp case, normal forward progression, and backward-time detection paths.

  if (got.imu && check_time_going_forwards()) {

[avtoku]

Looks good, but the test board needs to be updated.