Safety checking for suspect estimates and controller errors

src/comms/config_data/state.hpp

@@ -25,33 +25,41 @@ enum class StateOrder : uint32_t {
     Velocity,
     Acceleration,
 };
-/// @brief ReferenceLimit represents the limits for a state.
-struct ReferenceLimit {
+
+/// @brief Generic Limit type with a min and a max.
+struct Limit {
     /// @brief The minimum value
     float min = 0.0;
     /// @brief The maximum value
     float max = 0.0;
 };
 
-/// @brief ReferenceLimits represents the limits for all orders of a state. States are governed by these values.
-struct ReferenceLimits {
+/// @brief StateLimit represents the limits for all orders of a state. States are governed by these values.
+struct StateLimit {
     /// @brief The limits for the position order
-    ReferenceLimit position;
+    Limit position;
     /// @brief The limits for the velocity order
-    ReferenceLimit velocity;
+    Limit velocity;
     /// @brief The limits for the acceleration order
-    ReferenceLimit acceleration;
+    Limit acceleration;
 };
+
 /// @brief The `State` struct represents the configuration for a state, including its reference limits, governor type, whether it is wrapping, and its name.
 struct State : Comms::CommsData {
     /// @brief The reference limits for this state. The reference governor uses these values to determine how to limit the reference state that the controllers are trying to achieve.
-    ReferenceLimits reference_limits;
+    StateLimit reference_limits;
+    /// @brief The physical limits for this state. 
+    StateLimit physical_limits;
     /// @brief The governor type for this state. This determines whether the reference governor limits the position, velocity, or acceleration reference for this state.
     StateOrder governor_type = StateOrder::Position;
     /// @brief whether this state's position value wraps between the position reference limits.
     uint32_t is_wrapping = 0;
     /// @brief Name of this state
     StateName name = StateName::UnsetStateName;
+    /// @brief The maximum acceptable error between the estimate and target for this state before entering safety mode
+    float max_controller_error = 0.0f;
+    /// @brief The maximum time in microseconds that a check (controller error or estimator state violation) can exceed its limit before entering safety mode
+    uint32_t max_error_exceed_time_us = 0;
 };
 
 };

src/controls/controller.cpp

@@ -2,6 +2,128 @@
 #include "motor.hpp"
 #include "sensors/RefSystem.hpp"
 
+namespace {
+/// @brief Unwrap a potentially wrapped error value to maintain continuity across wrap boundaries.
+/// @param error The current error value.
+/// @param previous_error The previous control loop error value.
+/// @param config The state configuration containing wrap range information.
+/// @return The unwrapped error maintaining continuity from the previous value.
+float unwrap_controller_error(float error, float previous_error, const Cfg::State& config) {
+    if (!config.is_wrapping || config.governor_type != Cfg::StateOrder::Position) {
+        // Wrapping only applies to position control, and only if the state is configured to wrap
+        return error;
+    }
+
+    const float wrap_range = config.reference_limits.position.max - config.reference_limits.position.min;
+    if (wrap_range <= 0.0f) {
+        return error;
+    }
+
+    const float delta = error - previous_error;
+    const float half_wrap_range = wrap_range * 0.5f;
+    if (delta > half_wrap_range) {
+        error -= wrap_range;
+    } else if (delta < -half_wrap_range) {
+        error += wrap_range;
+    }
+
+    return error;
+}
+
+/// @brief Normalize a wrapped error value into its canonical form within the wrap range.
+/// @param error The error value to normalize.
+/// @param config The state configuration containing wrap range information.
+/// @return The normalized error value constrained to the wrapped position range.
+float normalize_wrapped_error(float error, const Cfg::State& config) {
+    if (!config.is_wrapping || config.governor_type != Cfg::StateOrder::Position) {
+        // Wrapping only applies to position control, and only if the state is configured to wrap
+        return error;
+    }
+
+    const float wrap_range = config.reference_limits.position.max - config.reference_limits.position.min;
+    if (wrap_range <= 0.0f) {
+        return error;
+    }
+
+    const float half_wrap_range = wrap_range * 0.5f;
+    while (error > half_wrap_range) {
+        error -= wrap_range;
+    }
+    while (error < -half_wrap_range) {
+        error += wrap_range;
+    }
+
+    return error;
+}
+} // namespace
+
+void Controller::checkControllerError(const char* controller_name, const char* state_name, const State& reference_state, const State& estimate_state, ErrorMonitor& monitor) {
+    // Skip error checking if in safety mode (robot is not actively controlled)
+    if (safety::is_safety_mode_active()) {
+        // Reset monitor to clean slate when safety mode is active
+        monitor.exceeding = false;
+        monitor.exceed_start_us = 0;
+        monitor.initialized = false;
+        monitor.previous_error = 0.0f;
+        return;
+    }
+
+    const Cfg::State& config = reference_state.config();
+    const float reference_value = get_state_error_value(reference_state);
+    const float estimate_value = get_state_error_value(estimate_state);
+
+    // Non-finite values indicate broken sensing/state propagation and should fail safe immediately.
+    if (!std::isfinite(reference_value) || !std::isfinite(estimate_value)) {
+        handleControllerError(controller_name, state_name, reference_state, estimate_state, reference_value - estimate_value);
+        return;
+    }
+
+    float error = reference_value - estimate_value;
+    if (!monitor.initialized) {
+        monitor.initialized = true;
+        monitor.previous_error = normalize_wrapped_error(error, config);
+        error = monitor.previous_error;
+    } else {
+        error = unwrap_controller_error(error, monitor.previous_error, config);
+        monitor.previous_error = error;
+    }
+
+    const float abs_error = fabsf(error);
+    if (abs_error > config.max_controller_error) {
+        if (!monitor.exceeding) {
+            monitor.exceeding = true;
+            monitor.exceed_start_us = micros();
+        }
+
+        // If error exceeds the configured threshold for too long, trigger the error handler
+        const uint32_t elapsed_us = static_cast<uint32_t>(micros() - monitor.exceed_start_us);
+        if (elapsed_us >= config.max_error_exceed_time_us) {
+            handleControllerError(controller_name, state_name, reference_state, estimate_state, error);
+        }
+    } else {
+        monitor.exceeding = false;
+        monitor.exceed_start_us = 0;
+    }
+}
+
+void Controller::handleControllerError(const char* controller_name, const char* state_name, const State& reference_state, const State& estimate_state, float error) {
+    const Cfg::State& config = reference_state.config();
+    safety::safety_procedure(
+        "%s: %s controller error exceeded threshold. error=%f threshold=%f",
+        controller_name,
+        state_name,
+        error,
+        config.max_controller_error
+    );
+}
+
+void XDriveController::validate(const RobotStateMap& reference_map, const RobotStateMap& estimate_map) {
+    // Our estimates of the chassis states are not accurate enough to run checks against their references.
+    // checkControllerError("XDriveController", "Chassis X", reference_map[chassis_x_state], estimate_map[chassis_x_state], chassis_x_error_monitor);
+    // checkControllerError("XDriveController", "Chassis Y", reference_map[chassis_y_state], estimate_map[chassis_y_state], chassis_y_error_monitor);
+    // checkControllerError("XDriveController", "Chassis Heading", reference_map[chassis_heading_state], estimate_map[chassis_heading_state], chassis_heading_error_monitor);
+}
+
 void XDriveController::step(RobotStateMap& reference_map, RobotStateMap& estimate_map) {
     float dt = timer.delta();
 
@@ -163,6 +285,14 @@ void XDriveController::step(RobotStateMap& reference_map, RobotStateMap& estimat
     }
 }
 
+void XDriveController::handleControllerError(const char* controller_name, const char* state_name, const State& reference_state, const State& estimate_state, float error) {
+    Controller::handleControllerError(controller_name, state_name, reference_state, estimate_state, error);
+}
+
+void YawController::validate(const RobotStateMap& reference_map, const RobotStateMap& estimate_map) {
+    checkControllerError("YawController", "Gimbal Yaw", reference_map[yaw_angle_state], estimate_map[yaw_angle_state], yaw_error_monitor);
+}
+
 void YawController::step(RobotStateMap& reference_map, RobotStateMap& estimate_map) {
     float dt = timer.delta();
     float output = 0.0;
@@ -190,13 +320,23 @@ void YawController::step(RobotStateMap& reference_map, RobotStateMap& estimate_m
 
     float motor_outputs[2];
 
+    // output = output * 0.1;
+
     motor_outputs[0] = controller_config.gear_ratios.motor1_direction * output;
     motor_outputs[1] = controller_config.gear_ratios.motor2_direction * output;
 
     yaw_motor_1->write_motor_torque(motor_outputs[0]);
     yaw_motor_2->write_motor_torque(motor_outputs[1]);
 }
 
+void YawController::handleControllerError(const char* controller_name, const char* state_name, const State& reference_state, const State& estimate_state, float error) {
+    Controller::handleControllerError(controller_name, state_name, reference_state, estimate_state, error);
+}
+
+void PitchController::validate(const RobotStateMap& reference_map, const RobotStateMap& estimate_map) {
+    checkControllerError("PitchController", "Gimbal Pitch", reference_map[pitch_angle_state], estimate_map[pitch_angle_state], pitch_error_monitor);
+}
+
 void PitchController::step(RobotStateMap& reference_map, RobotStateMap& estimate_map) {
     float dt = timer.delta();
     float output = 0.0;
@@ -230,6 +370,14 @@ void PitchController::step(RobotStateMap& reference_map, RobotStateMap& estimate
     pitch_motor_2->write_motor_torque(motor_outputs[1]);
 }
 
+void PitchController::handleControllerError(const char* controller_name, const char* state_name, const State& reference_state, const State& estimate_state, float error) {
+    Controller::handleControllerError(controller_name, state_name, reference_state, estimate_state, error);
+}
+
+void FlywheelController::validate(const RobotStateMap& reference_map, const RobotStateMap& estimate_map) {
+    // checkControllerError("FlywheelController", "Flywheels", reference_map[flywheel_velocity_state], estimate_map[flywheel_velocity_state], flywheel_error_monitor);
+}
+
 void FlywheelController::step(RobotStateMap& reference_map, RobotStateMap& estimate_map) {
     float dt = timer.delta();
 
@@ -263,6 +411,14 @@ void FlywheelController::step(RobotStateMap& reference_map, RobotStateMap& estim
     flywheel_motor_2->write_motor_torque(motor_outputs[1]);
 }
 
+void FlywheelController::handleControllerError(const char* controller_name, const char* state_name, const State& reference_state, const State& estimate_state, float error) {
+    Controller::handleControllerError(controller_name, state_name, reference_state, estimate_state, error);
+}
+
+void FeederController::validate(const RobotStateMap& reference_map, const RobotStateMap& estimate_map) {
+    // checkControllerError("FeederController", "Feeder", reference_map[feeder_position_state], estimate_map[feeder_position_state], feeder_error_monitor);
+}
+
 void FeederController::step(RobotStateMap& reference_map, RobotStateMap& estimate_map) {
     float dt = timer.delta();
     pidp.kp = full_state_position_controller.gains.p;
@@ -284,6 +440,14 @@ void FeederController::step(RobotStateMap& reference_map, RobotStateMap& estimat
    feeder_motor->write_motor_torque(output);
 }
 
+void FeederController::handleControllerError(const char* controller_name, const char* state_name, const State& reference_state, const State& estimate_state, float error) {
+    Controller::handleControllerError(controller_name, state_name, reference_state, estimate_state, error);
+}
+
+void LowerFeederController::validate(const RobotStateMap& reference_map, const RobotStateMap& estimate_map) {
+    // checkControllerError("LowerFeederController", "Lower Feeder", reference_map[lower_feeder_position_state], estimate_map[lower_feeder_position_state], lower_feeder_error_monitor);
+}
+
 void LowerFeederController::step(RobotStateMap& reference_map, RobotStateMap& estimate_map) {
     float dt = timer.delta();
     pidp.kp = full_state_position_controller.gains.p;