Fix ESC test functions

Author: Apehaenger

robots/include/sabo_robot.hpp

@@ -7,6 +7,7 @@
 #include "robot.hpp"
 
 using namespace xbot::driver::ui;
+using namespace xbot::driver::motor;
 
 class SaboRobot : public MowerRobot {
  public:
@@ -40,10 +41,23 @@ class SaboRobot : public MowerRobot {
     return 7.0f * 3.0;
   }
 
+  // ----- Some driver Test* functions used by Boot-Screen -----
+
   CHARGER_STATUS GetChargerStatus() {
     return charger_.getChargerStatus();
   }
 
+  bool TestESC(VescDriver& motor_driver);
+  bool TestLeftESC() {
+    return TestESC(left_motor_driver_);
+  }
+  bool TestRightESC() {
+    return TestESC(right_motor_driver_);
+  }
+  bool TestMowerESC() {
+    return TestESC(mower_motor_driver_);
+  }
+
  private:
   BQ2576 charger_{};
   SaboCoverUIController cover_ui_{};

robots/src/sabo_robot.cpp

@@ -44,3 +44,15 @@ bool SaboRobot::IsHardwareSupported() {
 
   return false;
 }
+
+bool SaboRobot::TestESC(VescDriver& motor_driver) {
+  if (!motor_driver.IsStarted()) return false;
+
+  if (motor_driver.GetLatestState().status == MotorDriver::ESCState::ESCStatus::ESC_STATUS_DISCONNECTED) {
+    motor_driver.RequestStatus();
+    // Give ESC driver some time to respond so that callee don't need to go into retry loop
+    chThdSleepMilliseconds(100);
+  }
+
+  return motor_driver.GetLatestState().status == MotorDriver::ESCState::ESCStatus::ESC_STATUS_OK;
+}

src/drivers/motor/motor_driver.hpp

@@ -55,11 +55,15 @@ class MotorDriver {
     state_callback_ = function;
   }
 
- protected:
   bool IsStarted() {
     return started_;
   }
 
+  ESCState GetLatestState() const {
+    return latest_state_;
+  }
+
+ protected:
   void NotifyCallback() {
     chDbgAssert(started_, "Notify can only be called after driver started.");
     if (state_callback_) {

src/drivers/ui/SaboCoverUI/sabo_cover_ui_controller.cpp

@@ -8,6 +8,7 @@
 #include <services.hpp>
 
 #include "chprintf.h"
+#include "globals.hpp"
 #include "hal.h"
 #include "robots/include/sabo_robot.hpp"
 #include "sabo_cover_ui_display.hpp"
@@ -220,18 +221,21 @@ bool SaboCoverUIController::TestGPS() {
 }
 
 bool SaboCoverUIController::TestLeftESC() {
-  auto esc_state = diff_drive.GetLeftESCState();
-  return esc_state.status == MotorDriver::ESCState::ESCStatus::ESC_STATUS_OK;
+  if (!robot) return false;
+  auto* sabo = static_cast<SaboRobot*>(robot);
+  return sabo->TestLeftESC();
 }
 
 bool SaboCoverUIController::TestRightESC() {
-  auto esc_state = diff_drive.GetRightESCState();
-  return esc_state.status == MotorDriver::ESCState::ESCStatus::ESC_STATUS_OK;
+  if (!robot) return false;
+  auto* sabo = static_cast<SaboRobot*>(robot);
+  return sabo->TestRightESC();
 }
 
 bool SaboCoverUIController::TestMowerESC() {
-  auto esc_state = mower_service.GetESCState();
-  return esc_state.status == MotorDriver::ESCState::ESCStatus::ESC_STATUS_OK;
+  if (!robot) return false;
+  auto* sabo = static_cast<SaboRobot*>(robot);
+  return sabo->TestMowerESC();
 }
 
 void SaboCoverUIController::ThreadFunc() {

src/drivers/ui/SaboCoverUI/sabo_cover_ui_controller.hpp

@@ -44,15 +44,14 @@ class SaboCoverUIController {
     BootStepState state = BootStepState::WAIT;
     systime_t last_action_time = 0;
   };
-  static constexpr size_t BOOT_STEP_COUNT_ = 7;
+  static constexpr size_t BOOT_STEP_COUNT_ = 6;
   etl::array<SaboCoverUIController::BootStep, BOOT_STEP_COUNT_> boot_steps_ = {{
       {"Motion Sensor", &TestIMU},
       {"Charger", &TestCharger},
       {"GPS", &TestGPS},
       {"Left Motor", &TestLeftESC},
       {"Right Motor", &TestRightESC},
       {"Mower Motor", &TestMowerESC},
-      {"Flux Compensator", &TestFluxCompensator},
   }};
   size_t current_boot_step_ = 0;
   static constexpr size_t BOOT_STEP_RETRIES = 3;
@@ -66,9 +65,6 @@ class SaboCoverUIController {
   static bool TestLeftESC();
   static bool TestRightESC();
   static bool TestMowerESC();
-  static bool TestFluxCompensator() {
-    return false;  // Placeholder for a non-existent service
-  }
 
   static void ThreadHelper(void* instance);
   void ThreadFunc();

src/services/diff_drive_service/diff_drive_service.hpp

@@ -43,14 +43,6 @@ class DiffDriveService : public DiffDriveServiceBase {
 
   void OnEmergencyChangedEvent();
 
-  MotorDriver::ESCState GetLeftESCState() const {
-    return left_esc_state_;
-  }
-
-  MotorDriver::ESCState GetRightESCState() const {
-    return right_esc_state_;
-  }
-
   void SetDrivers(MotorDriver *left_driver, MotorDriver *right_driver);
 
  protected: