1 files changed, 27 insertions, 3 deletions
diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
index 02c26709..f373ce41 100644
--- a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
+++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
@@ -28,6 +28,7 @@ constexpr float GyroOffsetOtcCal::kHighQualityRps;
 
 void GyroOffsetOtcCal::Initialize(const GyroCalParameters& gyro_cal_parameters,
                                   const OverTempCalParameters& otc_parameters) {
+  gyro_is_enabled_ = true;
   gyroCalInit(&gyro_cal_, &gyro_cal_parameters);
   overTempCalInit(&over_temp_cal_, &otc_parameters);
   InitializeCalData();
@@ -35,6 +36,17 @@ void GyroOffsetOtcCal::Initialize(const GyroCalParameters& gyro_cal_parameters,
 
 CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement(
     const SensorData& sample) {
+  // Bypass calibration data process and updates when the gyro sensor is not
+  // enabled.
+  if (!gyro_is_enabled_) {
+    // Tracks any updates in temperature.
+    if (sample.type == SensorType::kTemperatureCelsius) {
+      temperature_celsius_ = sample.data[SensorIndex::kSingleAxis];
+    }
+
+    return CalibrationTypeFlags::NONE;
+  }
+
   // Routes the input sensor sample to the calibration algorithm.
   switch (sample.type) {
     case SensorType::kAccelerometerMps2:
@@ -78,9 +90,12 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement(
     uint64_t calibration_time_nanos = 0;
     gyroCalGetBias(&gyro_cal_, &offset[0], &offset[1], &offset[2],
                    &temperature_celsius, &calibration_time_nanos);
-    overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos,
-                                    offset, temperature_celsius);
-    cal_update_callback_flags |= CalibrationTypeFlags::OTC_STILL_BIAS;
+
+    if (temperature_celsius != kInvalidTemperatureCelsius) {
+      overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos,
+                                      offset, temperature_celsius);
+      cal_update_callback_flags |= CalibrationTypeFlags::OTC_STILL_BIAS;
+    }
   }
 
   // Checks the OTC for a new calibration model update.
@@ -178,4 +193,13 @@ bool GyroOffsetOtcCal::SetInitialCalibration(
   return true;
 }
 
+void GyroOffsetOtcCal::UpdateSensorEnableState(SensorType sensor_type,
+                                               uint8_t sensor_index,
+                                               bool is_enabled) {
+  if (sensor_type == SensorType::kGyroscopeRps &&
+      sensor_index_ == sensor_index) {
+    gyro_is_enabled_ = is_enabled;
+  }
+}
+
 }  // namespace online_calibration