feat(include): add checks for exceeding max sensor readings for the p…

…ressure and cap sensor (#672)

This is only really relevant to the pressure sensor, but I decided to add support for the cap sensor
as well. We want to make sure that the sensor does not ever exceed its max output to ensure that the
sensor itself doesn't break.


* feat(include): add error code for overpressure trigger (#673)

We should throw an error message for the use-case of an overpressure spike being detected.
Potentially we'll want to do some filtering at some point, but I think the chances of us getting
into this value range is very low and thus it's OK to not do any type of signal processing.

include/bootloader/core/ids.h

@@ -145,6 +145,7 @@ typedef enum {
     can_errorcode_estop_released = 0xa,
     can_errorcode_motor_busy = 0xb,
     can_errorcode_stop_requested = 0xc,
+    can_errorcode_over_pressure = 0xd,
 } CANErrorCode;
 
 /** Tool types detected on Head. */
@@ -181,6 +182,7 @@ typedef enum {
     can_sensoroutputbinding_none = 0x0,
     can_sensoroutputbinding_sync = 0x1,
     can_sensoroutputbinding_report = 0x2,
+    can_sensoroutputbinding_max_threshold_sync = 0x4,
 } CANSensorOutputBinding;
 
 /** How a sensor's threshold should be interpreted. */

include/can/core/ids.hpp

@@ -147,6 +147,7 @@ enum class ErrorCode {
     estop_released = 0xa,
     motor_busy = 0xb,
     stop_requested = 0xc,
+    over_pressure = 0xd,
 };
 
 /** Error Severity levels. */
@@ -190,6 +191,7 @@ enum class SensorOutputBinding {
     none = 0x0,
     sync = 0x1,
     report = 0x2,
+    max_threshold_sync = 0x4,
 };
 
 /** How a sensor's threshold should be interpreted. */

include/sensors/core/fdc1004.hpp

@@ -29,6 +29,10 @@ namespace fdc1004 {
 
 constexpr uint16_t ADDRESS = 0x50 << 1;
 
+// Max reading including the offset for the sensor is approximately
+// 115 pF.
+constexpr float MAX_CAPACITANCE_READING = 115.0F;
+
 enum class CHA : uint8_t {
     CIN1 = 0x0,
     CIN2 = 0x1,

include/sensors/core/mmr920C04.hpp

@@ -29,6 +29,10 @@ namespace sensors {
 namespace mmr920C04 {
 constexpr uint16_t ADDRESS = 0x67 << 1;
 
+// The pressure range is approximately 3922.0F. We shouldn't
+// need to ever exceed this value.
+constexpr float MAX_PRESSURE_READING = 3922.0F;
+
 enum class SensorStatus : uint8_t {
     SHUTDOWN = 0x0,
     IDLE = 0xE5,

include/sensors/core/tasks/capacitive_driver.hpp

@@ -80,6 +80,11 @@ class FDC1004 {
         hardware.reset_sync();
     }
 
+    void set_max_bind_sync(bool should_bind) {
+        max_capacitance_sync = should_bind;
+        hardware.reset_sync();
+    }
+
     auto set_bind_flags(uint8_t binding) -> void { sensor_binding = binding; }
 
     auto set_measurement_rate(fdc1004::MeasurementRate rate) -> void {
@@ -228,7 +233,7 @@ class FDC1004 {
             return;
         }
 
-        if (!bind_sync && !echoing) {
+        if (!bind_sync && !echoing && !max_capacitance_sync) {
             stop_continuous_polling(m.id.token);
             return;
         }
@@ -241,13 +246,21 @@ class FDC1004 {
         auto new_offset =
             fdc1004_utils::update_offset(capacitance, current_offset_pf);
         set_offset(new_offset);
+        if (max_capacitance_sync) {
+            if (capacitance > fdc1004::MAX_CAPACITANCE_READING) {
+                hardware.set_sync();
+            } else {
+                hardware.reset_sync();
+            }
+        }
         if (bind_sync) {
             if (capacitance > zero_threshold_pf) {
                 hardware.set_sync();
             } else {
                 hardware.reset_sync();
             }
         }
+
         if (echoing) {
             can_client.send_can_message(
                 can::ids::NodeId::host,
@@ -344,6 +357,7 @@ class FDC1004 {
     uint16_t number_of_reads = 1;
     bool echoing = false;
     bool bind_sync = false;
+    bool max_capacitance_sync = false;
     std::array<uint16_t, 2> baseline_results{};
     std::array<uint16_t, 2> polling_results{};
 

include/sensors/core/tasks/capacitive_sensor_task.hpp

@@ -144,6 +144,9 @@ class CapacitiveMessageHandler {
         driver.set_bind_sync(
             m.binding &
             static_cast<uint8_t>(can::ids::SensorOutputBinding::sync));
+        driver.set_max_bind_sync(
+            m.binding &
+            static_cast<uint8_t>(can::ids::SensorOutputBinding::sync));
         std::array tags{utils::ResponseTag::IS_PART_OF_POLL,
                         utils::ResponseTag::POLL_IS_CONTINUOUS};
         auto tags_as_int = utils::byte_from_tags(tags);

include/sensors/core/tasks/pressure_driver.hpp

@@ -69,6 +69,11 @@ class MMR920C04 {
         hardware.reset_sync();
     }
 
+    void set_max_bind_sync(bool should_bind) {
+        max_pressure_sync = should_bind;
+        hardware.reset_sync();
+    }
+
     auto get_threshold() -> int32_t { return threshold_pascals; }
 
     auto set_threshold(float threshold_pa,
@@ -265,7 +270,7 @@ class MMR920C04 {
 
     auto handle_ongoing_pressure_response(i2c::messages::TransactionResponse &m)
         -> void {
-        if (!bind_sync && !echoing) {
+        if (!bind_sync && !echoing && !max_pressure_sync) {
             auto reg_id = utils::reg_from_id<mmr920C04::Registers>(m.id.token);
             stop_continuous_polling(m.id.token, static_cast<uint8_t>(reg_id));
         }
@@ -280,6 +285,21 @@ class MMR920C04 {
         save_pressure(shifted_data_store);
         auto pressure = mmr920C04::PressureResult::to_pressure(
             _registers.pressure_result.reading);
+
+        if (max_pressure_sync) {
+            if (std::fabs(pressure) - std::fabs(current_pressure_baseline_pa) >
+                mmr920C04::MAX_PRESSURE_READING) {
+                hardware.set_sync();
+                can_client.send_can_message(
+                    can::ids::NodeId::host,
+                    can::messages::ErrorMessage{
+                        .message_index = m.message_index,
+                        .severity = can::ids::ErrorSeverity::unrecoverable,
+                        .error_code = can::ids::ErrorCode::over_pressure});
+            } else {
+                hardware.reset_sync();
+            }
+        }
         if (bind_sync) {
             if (std::fabs(pressure) - std::fabs(current_pressure_baseline_pa) >
                 threshold_pascals) {
@@ -433,14 +453,17 @@ class MMR920C04 {
     bool _initialized = false;
     bool echoing = false;
     bool bind_sync = false;
+    bool max_pressure_sync = false;
 
     float pressure_running_total = 0;
     float temperature_running_total = 0;
     uint16_t total_baseline_reads = 1;
-    // TODO(fs, 2022-11-11): Need to figure out a realistic threshold. Pretty
-    // sure this is an arbitrarily large number to enable continuous reads.
+
     float current_pressure_baseline_pa = 0;
     float current_temperature_baseline = 0;
+
+    // TODO(fs, 2022-11-11): Need to figure out a realistic threshold. Pretty
+    // sure this is an arbitrarily large number to enable continuous reads.
     float threshold_pascals = 100.0F;
     float offset_average = 0;
 

include/sensors/core/tasks/pressure_sensor_task.hpp

@@ -124,6 +124,9 @@ class PressureMessageHandler {
         driver.set_bind_sync(
             m.binding &
             static_cast<uint8_t>(can::ids::SensorOutputBinding::sync));
+        driver.set_max_bind_sync(
+            m.binding & static_cast<uint8_t>(
+                            can::ids::SensorOutputBinding::max_threshold_sync));
         std::array tags{utils::ResponseTag::IS_PART_OF_POLL,
                         utils::ResponseTag::POLL_IS_CONTINUOUS};
         auto tags_as_int = utils::byte_from_tags(tags);