From cc7bbd47f88615decefda5afd156f77202beec85 Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Wed, 30 Mar 2022 18:16:17 +0200
Subject: [PATCH] add back subcomponent error flags

These flags were previously removed in 5661a3b5ed93591d3bfcd81dc355e8cd4079e2d7
---
 Arduino/ODriveArduino/ODriveEnums.h            | 4 ++++
 Firmware/MotorControl/encoder.cpp              | 1 +
 Firmware/MotorControl/main.cpp                 | 7 +++++--
 Firmware/MotorControl/motor.cpp                | 1 +
 Firmware/MotorControl/sensorless_estimator.cpp | 2 ++
 Firmware/odrive-interface.yaml                 | 7 +++++++
 tools/odrive/enums.py                          | 4 ++++
 7 files changed, 24 insertions(+), 2 deletions(-)

diff --git a/Arduino/ODriveArduino/ODriveEnums.h b/Arduino/ODriveArduino/ODriveEnums.h
index f196ecbd..627c7b41 100644
--- a/Arduino/ODriveArduino/ODriveEnums.h
+++ b/Arduino/ODriveArduino/ODriveEnums.h
@@ -120,6 +120,10 @@ enum CanError {
 enum AxisError {
     AXIS_ERROR_NONE                          = 0x00000000,
     AXIS_ERROR_INVALID_STATE                 = 0x00000001,
+    AXIS_ERROR_MOTOR_FAILED                  = 0x00000040,
+    AXIS_ERROR_SENSORLESS_ESTIMATOR_FAILED   = 0x00000080,
+    AXIS_ERROR_ENCODER_FAILED                = 0x00000100,
+    AXIS_ERROR_CONTROLLER_FAILED             = 0x00000200,
     AXIS_ERROR_WATCHDOG_TIMER_EXPIRED        = 0x00000800,
     AXIS_ERROR_MIN_ENDSTOP_PRESSED           = 0x00001000,
     AXIS_ERROR_MAX_ENDSTOP_PRESSED           = 0x00002000,
diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp
index 000e6b64..73771c34 100644
--- a/Firmware/MotorControl/encoder.cpp
+++ b/Firmware/MotorControl/encoder.cpp
@@ -70,6 +70,7 @@ void Encoder::set_error(Error error) {
     vel_estimate_valid_ = false;
     pos_estimate_valid_ = false;
     error_ |= error;
+    axis_->error_ |= Axis::ERROR_ENCODER_FAILED;
 }
 
 bool Encoder::do_checks(){
diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp
index e859c812..959a62bf 100644
--- a/Firmware/MotorControl/main.cpp
+++ b/Firmware/MotorControl/main.cpp
@@ -439,8 +439,11 @@ void ODrive::control_loop_cb(uint32_t timestamp) {
         MEASURE_TIME(axis.task_times_.sensorless_estimator_update)
             axis.sensorless_estimator_.update();
 
-        MEASURE_TIME(axis.task_times_.controller_update)
-            axis.controller_.update(); // uses position and velocity from encoder
+        MEASURE_TIME(axis.task_times_.controller_update) {
+            if (!axis.controller_.update()) { // uses position and velocity from encoder
+                axis.error_ |= Axis::ERROR_CONTROLLER_FAILED;
+            }
+        }
 
         MEASURE_TIME(axis.task_times_.open_loop_controller_update)
             axis.open_loop_controller_.update(timestamp);
diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp
index d4429377..7c76f5d5 100644
--- a/Firmware/MotorControl/motor.cpp
+++ b/Firmware/MotorControl/motor.cpp
@@ -339,6 +339,7 @@ bool Motor::setup() {
 
 void Motor::disarm_with_error(Motor::Error error){
     error_ |= error;
+    axis_->error_ |= Axis::ERROR_MOTOR_FAILED;
     last_error_time_ = odrv.n_evt_control_loop_ * current_meas_period;
     disarm();
 }
diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp
index 94c09c68..3e6e4796 100644
--- a/Firmware/MotorControl/sensorless_estimator.cpp
+++ b/Firmware/MotorControl/sensorless_estimator.cpp
@@ -29,6 +29,7 @@ bool SensorlessEstimator::update() {
     // Check that we don't get problems with discrete time approximation
     if (!(current_meas_period * pll_kp < 1.0f)) {
         error_ |= ERROR_UNSTABLE_GAIN;
+        axis_->error_ |= Axis::ERROR_SENSORLESS_ESTIMATOR_FAILED;
         reset(); // Reset state for when the next valid current measurement comes in.
         return false;
     }
@@ -43,6 +44,7 @@ bool SensorlessEstimator::update() {
     }
     if (!current_meas.has_value()) {
         error_ |= ERROR_UNKNOWN_CURRENT_MEASUREMENT;
+        axis_->error_ |= Axis::ERROR_SENSORLESS_ESTIMATOR_FAILED;
         reset(); // Reset state for when the next valid current measurement comes in.
         return false;
     }
diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml
index 85b35e2a..2f9da82b 100644
--- a/Firmware/odrive-interface.yaml
+++ b/Firmware/odrive-interface.yaml
@@ -446,6 +446,13 @@ interfaces:
               tried to run encoder calibration or closed loop control before the
               motor was calibrated, or you tried to run closed loop control
               before the encoder was calibrated.
+          MOTOR_FAILED:
+            bit: 6
+            doc: Check `motor.error` for more information.
+          SENSORLESS_ESTIMATOR_FAILED:
+          ENCODER_FAILED:
+            doc: Check `encoder.error` for more information.
+          CONTROLLER_FAILED:
           WATCHDOG_TIMER_EXPIRED:
             bit: 11
             brief: The axis watchdog timer expired.
diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py
index 292ee3be..5feb5423 100644
--- a/tools/odrive/enums.py
+++ b/tools/odrive/enums.py
@@ -95,6 +95,10 @@ CAN_ERROR_DUPLICATE_CAN_IDS              = 0x00000001
 # ODrive.Axis.Error
 AXIS_ERROR_NONE                          = 0x00000000
 AXIS_ERROR_INVALID_STATE                 = 0x00000001
+AXIS_ERROR_MOTOR_FAILED                  = 0x00000040
+AXIS_ERROR_SENSORLESS_ESTIMATOR_FAILED   = 0x00000080
+AXIS_ERROR_ENCODER_FAILED                = 0x00000100
+AXIS_ERROR_CONTROLLER_FAILED             = 0x00000200
 AXIS_ERROR_WATCHDOG_TIMER_EXPIRED        = 0x00000800
 AXIS_ERROR_MIN_ENDSTOP_PRESSED           = 0x00001000
 AXIS_ERROR_MAX_ENDSTOP_PRESSED           = 0x00002000