IMUs will become bad after 50ms

I had false positives happen with 10 and 20ms, but I could not replicate it with 30ms.  To be safe this is 50ms.

The calibrate IMU function can remove good IMUs if one started unplugged.

include/EZ-Template/drive/drive.hpp

@@ -3460,7 +3460,7 @@ class Drive {
 
  private:
   std::map<int, double> imu_scale_map = {};
-  std::map<int, double> prev_imu_values = {};
+  std::map<int, std::pair<double, int>> prev_imu_values = {};
   void opcontrol_drive_activebrake_targets_set();
   double odom_smooth_weight_smooth = 0.0;
   double odom_smooth_weight_data = 0.0;

src/EZ-Template/drive/drive.cpp

@@ -455,15 +455,17 @@ void Drive::drive_imu_display_loading(int iter) {
 
 bool Drive::drive_imu_calibrate(bool run_loading_animation) {
   imu_calibration_complete = false;
+  bool one_calibrated = false;
 
   // No IMUs are calibrated yet, set them all to false
-  std::vector<bool> imus_status, imus_done;
+  std::map<int, bool> imus_status, imus_done, imus_last_status;
   for (int i = 0; i < good_imus.size(); i++) {
     good_imus[i]->reset();
-    imus_status.push_back(good_imus[i]->is_calibrating());
-    imus_done.push_back(false);
+    int port = good_imus[i]->get_port();
+    imus_status[port] = good_imus[i]->is_calibrating();
+    imus_last_status[port] = imus_status[port];
+    imus_done[port] = false;
   }
-  std::vector<bool> imus_last_status = imus_status;
 
   bool successful = false;
   int iter = 0;
@@ -475,38 +477,68 @@ bool Drive::drive_imu_calibrate(bool run_loading_animation) {
     if (!successful) {
       // Check if each IMU is done calibrating
       for (int i = 0; i < good_imus.size(); i++) {
-        imus_last_status[i] = imus_status[i];
-        imus_status[i] = good_imus[i]->is_calibrating();
-        if (!imus_done[i])
-          imus_done[i] = !imus_status[i] && imus_last_status[i] ? true : false;
+        int port = good_imus[i]->get_port();
+        imus_last_status[port] = imus_status[port];
+        imus_status[port] = good_imus[i]->is_calibrating();
+        if (!imus_done[port])
+          imus_done[port] = !imus_status[port] && imus_last_status[port] ? true : false;
       }
     }
 
     // Check if all IMUs have calibrated
     successful = true;
-    for (auto i : imus_done) {
-      if (!i) {
+    for (int i = 0; i < good_imus.size(); i++) {
+      if (!imus_done[good_imus[i]->get_port()]) {
         successful = false;
-        break;
+      } else {
+        one_calibrated = true;  // Remember that at least 1 IMU has calibrated
       }
     }
 
     if (iter >= 2000) {
       if (successful) {
+        printf("IMU is done calibrating (took %d ms)\n", iter);
+        imu_calibration_complete = true;
+        imu_calibrate_took_too_long = iter > 2000 ? true : false;
         break;
       }
       if (iter >= 3000) {
-        printf("No IMU plugged in, (took %d ms to realize that)\n", iter);
+        if (!one_calibrated) {
+          printf("No IMU plugged in");
+        } else {
+          printf("Only IMUs in ports {");
+          for (int i = 0; i < good_imus.size(); i++) {
+            int port = good_imus[i]->get_port();
+            if (imus_done[port])
+              printf(" %i", port);
+          }
+          printf(" } calibrated");
+        }
+        printf(", (took %d ms to realize that)\n", iter);
         imu_calibrate_took_too_long = true;
-        return false;
+        break;
       }
     }
     pros::delay(util::DELAY_TIME);
   }
-  printf("IMU is done calibrating (took %d ms)\n", iter);
-  imu_calibration_complete = true;
-  imu_calibrate_took_too_long = iter > 2000 ? true : false;
-  return true;
+
+  // Run through all of the IMUs and remove any IMUs that didn't calibrate successfully
+  for (int i = 0; i < good_imus.size(); i++) {
+    int port = good_imus[i]->get_port();
+
+    if (!imus_done[port]) {
+      good_imus.erase(good_imus.begin() + i);
+      if (i == 0 && !good_imus.empty())
+        imu = good_imus.front();
+    }
+  }
+
+  if (one_calibrated && !good_imus.empty())
+    imu_calibration_complete = true;
+
+  printf("one cali-%i\n", one_calibrated);
+
+  return imu_calibration_complete;
 }
 
 bool Drive::drive_imu_calibrated() {

src/EZ-Template/drive/maintenance.cpp

@@ -14,11 +14,17 @@ void Drive::check_imu_task() {
   // Don't let this function run if IMU calibration is incomplete
   if (!imu_calibration_complete) return;
 
-  // erase indices only if imu val equals previous one
-  good_imus.erase(std::remove_if(good_imus.begin(), good_imus.end(), [this](pros::Imu *n) { return n->get_status() == pros::ImuStatus::error || errno == PROS_ERR || get_this_imu(n) == prev_imu_values[n->get_port()]; }), good_imus.end());
+  // Erase indices only if imu val equals previous one
+  good_imus.erase(std::remove_if(good_imus.begin(), good_imus.end(), [this](pros::Imu *n) { return n->get_status() == pros::ImuStatus::error /*|| errno == PROS_ERR*/ || prev_imu_values[n->get_port()].second >= 5; }), good_imus.end());
 
-  for (size_t i = 0; i < good_imus.size(); i++)
-    prev_imu_values[good_imus[i]->get_port()] = get_this_imu(good_imus[i]);
+  // Increment every time an IMU doesn't update
+  for (size_t i = 0; i < good_imus.size(); i++) {
+    if (prev_imu_values[good_imus[i]->get_port()].first == get_this_imu(good_imus[i]))
+      prev_imu_values[good_imus[i]->get_port()].second += 1;
+    else
+      prev_imu_values[good_imus[i]->get_port()].second = 0;
+    prev_imu_values[good_imus[i]->get_port()].first = get_this_imu(good_imus[i]);
+  }
 
   if (!good_imus.empty())
     imu = good_imus.front();