bug: fixed state_.timestamp in estimator. Controller was reading non-initialized memory

Author: JMoore5353

src/controller.cpp

@@ -118,6 +118,7 @@ void Controller::run()
   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);
+    prev_time_us_ = RF_.estimator_.state().timestamp_us;
     return;
   }
   prev_time_us_ = RF_.estimator_.state().timestamp_us;

src/estimator.cpp

@@ -130,6 +130,7 @@ void Estimator::run()
     last_time_ = now_us;
     last_acc_update_us_ = now_us;
     last_extatt_update_us_ = now_us;
+    state_.timestamp_us = now_us;
     return;
   } else if (now_us < last_time_) {
     // this shouldn't happen