fix(sensors): mitigate cap sensor configuration issues (#703)

fsinapi · web-flow · commit 6d7b2a492d07 · 2023-07-13T12:13:38.000-04:00
* Fixes for FDC1004
- Check the Ready flag for each channel before reading data
- Only set the sample rate ONE time ever
- Add some delays during capsense initialization to avoid all of the I2C bus traffic
- Take out the filtering stuff
* Cleaned up tests
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -69,6 +69,8 @@ if (${CMAKE_CROSSCOMPILING})
 
     add_compile_definitions(ENABLE_CCMRAM)
 
+    add_compile_definitions(ENABLE_CROSS_ONLY_HEADERS)
+
     configure_file(common/firmware/platform_specific_hal_conf.h.in ${CMAKE_BINARY_DIR}/generated/platform_specific_hal_conf.h)
     configure_file(common/firmware/platform_specific_hal.h.in ${CMAKE_BINARY_DIR}/generated/platform_specific_hal.h)
 
diff --git a/include/sensors/core/fdc1004.hpp b/include/sensors/core/fdc1004.hpp
@@ -129,7 +129,7 @@ template <typename Reg>
 // This is used to mask the value before writing it to the sensor.
 concept FDC1004Register =
     std::same_as<std::remove_cvref_t<decltype(Reg::address)>,
-                 std::remove_cvref_t<Registers&>> &&
+                 std::remove_cvref_t<Registers &>> &&
     std::integral<decltype(Reg::value_mask)>;
 
 template <typename Reg>
@@ -559,5 +559,21 @@ inline auto update_offset(float capacitance_pf, float current_offset_pf)
 
     return static_cast<float>(capdac) * CAPDAC_PF_PER_LSB;
 }
+
+inline auto measurement_ready(fdc1004::FDCConf &fdc,
+                              fdc1004::MeasureConfigMode mode) -> bool {
+    switch (mode) {
+        case fdc1004::MeasureConfigMode::ONE:
+            return fdc.measure_mode_1_status;
+        case fdc1004::MeasureConfigMode::TWO:
+            return fdc.measure_mode_2_status;
+        case fdc1004::MeasureConfigMode::THREE:
+            return fdc.measure_mode_3_status;
+        case fdc1004::MeasureConfigMode::FOUR:
+            return fdc.measure_mode_4_status;
+    }
+    return false;
+}
+
 }  // namespace fdc1004_utils
 };  // namespace sensors
diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp
@@ -1,7 +1,21 @@
 #pragma once
 
 #include <array>
-#include <bitset>
+
+#ifdef ENABLE_CROSS_ONLY_HEADERS
+// TODO(fps 7/12/2023): This is super hacky and I hate throwing #ifdefs
+// in our nicely host-independent code but for now we really just need
+// the vTaskDelay function and hopefully sometime in the near future I
+// can refactor this file with a nice templated sleep function.
+#include "FreeRTOS.h"
+// NOLINTNEXTLINE(cppcoreguidelines-macro-usage)
+#define HACKY_TASK_SLEEP(___timeout___) vTaskDelay(___timeout___)
+
+#else
+
+// NOLINTNEXTLINE(cppcoreguidelines-macro-usage)
+#define HACKY_TASK_SLEEP(___timeout___) (void)(___timeout___)
+#endif
 
 #include "can/core/can_writer_task.hpp"
 #include "can/core/ids.hpp"
@@ -10,7 +24,6 @@
 #include "common/core/logging.h"
 #include "common/core/message_queue.hpp"
 #include "i2c/core/messages.hpp"
-#include "ot_utils/core/filters/sma.hpp"
 #include "sensors/core/fdc1004.hpp"
 #include "sensors/core/sensor_hardware_interface.hpp"
 #include "sensors/core/utils.hpp"
@@ -38,19 +51,23 @@ class FDC1004 {
     [[nodiscard]] auto initialized() const -> bool { return _initialized; }
 
     auto initialize() -> void {
-        // FIXME we should grab device id and compare it to the static
-        // device id in code.
-
-        // We should send a message that the sensor is in a ready state,
-        // not sure if we should have a separate can message to do that
-        // holding off for this PR.
-        if (shared_sensor) {
-            measure_mode = fdc1004::MeasureConfigMode::TWO;
+        if (!_initialized) {
+            // FIXME we should grab device id and compare it to the static
+            // device id in code.
+
+            // We should send a message that the sensor is in a ready state,
+            // not sure if we should have a separate can message to do that
+            // holding off for this PR.
+
+            // Initial delay to avoid I2C bus traffic.
+            HACKY_TASK_SLEEP(100);
             update_capacitance_configuration();
+            // Second delay to ensure IC is ready to start
+            // readings (and also to avoid I2C bus traffic).
+            HACKY_TASK_SLEEP(100);
+            set_sample_rate();
+            _initialized = true;
         }
-        measure_mode = fdc1004::MeasureConfigMode::ONE;
-        update_capacitance_configuration();
-        _initialized = true;
     }
 
     auto register_map() -> fdc1004::FDC1004RegisterMap & { return _registers; }
@@ -70,7 +87,6 @@ class FDC1004 {
             }
             sensor_id = _id;
             update_capacitance_configuration();
-            filter.reset_filter();
         }
     }
 
@@ -100,7 +116,6 @@ class FDC1004 {
         // mode.
         current_offset_pf = -1;
         set_offset(0);
-        set_sample_rate();
     }
 
     void reset_limited() {
@@ -116,13 +131,10 @@ class FDC1004 {
         _registers.fdc_conf.measurement_rate =
             static_cast<uint8_t>(measurement_rate);
         _registers.fdc_conf.repeating_measurements = 1;
-        if (measure_mode == fdc1004::MeasureConfigMode::TWO) {
-            _registers.fdc_conf.measure_mode_1 = 0;
-            _registers.fdc_conf.measure_mode_2 = 1;
-        } else {
-            _registers.fdc_conf.measure_mode_1 = 1;
-            _registers.fdc_conf.measure_mode_2 = 0;
-        }
+        // Enable both measurements no matter what. We check the Ready
+        // bits anyways and the data doesn't overwrite, so there's no danger.
+        _registers.fdc_conf.measure_mode_1 = 1;
+        _registers.fdc_conf.measure_mode_2 = 1;
         _registers.fdc_conf.padding_0 = 0;
         _registers.fdc_conf.padding_1 = 0;
         set_register(_registers.fdc_conf);
@@ -164,66 +176,51 @@ class FDC1004 {
     auto poll_limited_capacitance(uint16_t number_reads, can::ids::SensorId _id,
                                   uint8_t tags) -> void {
         set_sensor_id(_id);
-        auto converted_msb_addr =
-            static_cast<uint8_t>(fdc1004::Registers::MEAS1_MSB);
-        auto converted_lsb_addr =
-            static_cast<uint8_t>(fdc1004::Registers::MEAS1_LSB);
-        if (measure_mode == fdc1004::MeasureConfigMode::TWO) {
-            converted_msb_addr =
-                static_cast<uint8_t>(fdc1004::Registers::MEAS2_MSB);
-            converted_lsb_addr =
-                static_cast<uint8_t>(fdc1004::Registers::MEAS2_LSB);
-            poller.multi_register_poll(
-                fdc1004::ADDRESS, converted_msb_addr, 2, converted_lsb_addr, 2,
-                number_reads, DELAY, own_queue,
-                utils::build_id(fdc1004::ADDRESS, converted_msb_addr, tags));
-        } else {
-            poller.multi_register_poll(
-                fdc1004::ADDRESS, converted_msb_addr, 2, converted_lsb_addr, 2,
-                number_reads, DELAY, own_queue,
-                utils::build_id(fdc1004::ADDRESS, converted_msb_addr, tags));
-        }
+        poller.single_register_poll(
+            fdc1004::ADDRESS,
+            static_cast<uint8_t>(fdc1004::Registers::FDC_CONF), 2, number_reads,
+            DELAY, own_queue,
+            utils::build_id(fdc1004::ADDRESS,
+                            static_cast<uint8_t>(fdc1004::Registers::FDC_CONF),
+                            tags));
     }
 
     auto poll_continuous_capacitance(can::ids::SensorId _id, uint8_t tags,
                                      uint8_t binding) -> void {
         set_sensor_id(_id);
         auto delay = delay_or_disable(binding);
-        auto converted_msb_addr =
-            static_cast<uint8_t>(fdc1004::Registers::MEAS1_MSB);
-        auto converted_lsb_addr =
-            static_cast<uint8_t>(fdc1004::Registers::MEAS1_LSB);
-        if (measure_mode == fdc1004::MeasureConfigMode::TWO) {
-            converted_msb_addr =
-                static_cast<uint8_t>(fdc1004::Registers::MEAS2_MSB);
-            converted_lsb_addr =
-                static_cast<uint8_t>(fdc1004::Registers::MEAS2_LSB);
-            poller.continuous_multi_register_poll(
-                fdc1004::ADDRESS, converted_msb_addr, 2, converted_lsb_addr, 2,
-                delay, own_queue,
-                utils::build_id(fdc1004::ADDRESS, converted_msb_addr, tags));
-        } else {
-            poller.continuous_multi_register_poll(
-                fdc1004::ADDRESS, converted_msb_addr, 2, converted_lsb_addr, 2,
-                delay, own_queue,
-                utils::build_id(fdc1004::ADDRESS, converted_msb_addr, tags));
-        }
+        poller.continuous_single_register_poll(
+            fdc1004::ADDRESS,
+            static_cast<uint8_t>(fdc1004::Registers::FDC_CONF), 2, delay,
+            own_queue,
+            utils::build_id(fdc1004::ADDRESS,
+                            static_cast<uint8_t>(fdc1004::Registers::FDC_CONF),
+                            tags));
     }
 
     auto stop_continuous_polling(uint32_t transaction_id) -> void {
-        auto converted_msb_addr =
-            static_cast<uint8_t>(fdc1004::Registers::MEAS1_MSB);
-        auto converted_lsb_addr =
-            static_cast<uint8_t>(fdc1004::Registers::MEAS1_LSB);
-        if (measure_mode == fdc1004::MeasureConfigMode::TWO) {
-            converted_msb_addr =
-                static_cast<uint8_t>(fdc1004::Registers::MEAS2_MSB);
-            converted_lsb_addr =
-                static_cast<uint8_t>(fdc1004::Registers::MEAS2_LSB);
+        poller.continuous_single_register_poll(
+            fdc1004::ADDRESS,
+            static_cast<uint8_t>(fdc1004::Registers::FDC_CONF), 2, STOP_DELAY,
+            own_queue, transaction_id);
+    }
+
+    void handle_fdc_response(i2c::messages::TransactionResponse &m) {
+        uint16_t reg_int = 0;
+        static_cast<void>(bit_utils::bytes_to_int(
+            m.read_buffer.cbegin(), m.read_buffer.cend(), reg_int));
+        auto fdc = read_register<fdc1004::FDCConf>(reg_int).value();
+        auto measurement_done =
+            fdc1004_utils::measurement_ready(fdc, measure_mode);
+        if (measurement_done) {
+            // Start a single-shot, two-register transaction for the data.
+            send_followup(m);
+        } else {
+            i2c::messages::MaxMessageBuffer buffer = {
+                static_cast<uint8_t>(fdc1004::Registers::FDC_CONF)};
+            // Retrigger the same exact reading
+            writer.transact(fdc1004::ADDRESS, 1, buffer, 2, m.id, own_queue);
         }
-        poller.continuous_multi_register_poll(
-            fdc1004::ADDRESS, converted_msb_addr, 2, converted_lsb_addr, 2,
-            STOP_DELAY, own_queue, transaction_id);
     }
 
     void handle_ongoing_response(i2c::messages::TransactionResponse &m) {
@@ -233,6 +230,7 @@ class FDC1004 {
             // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
             polling_results[m.id.transaction_index]));
         if (m.id.transaction_index == 0) {
+            send_followup(m);
             return;
         }
 
@@ -244,19 +242,12 @@ class FDC1004 {
         auto raw_capacitance = fdc1004_utils::convert_reads(polling_results[0],
                                                             polling_results[1]);
 
-        if (!data_stable[int(sensor_id)]) {
-            raw_capacitance = filter.compute(raw_capacitance);
-            data_stable.set(int(sensor_id), filter.stop_filter());
-        }
-
         auto capacitance = fdc1004_utils::convert_capacitance(
             raw_capacitance, 1, current_offset_pf);
 
-        if (data_stable[int(sensor_id)]) {
-            auto new_offset =
-                fdc1004_utils::update_offset(capacitance, current_offset_pf);
-            set_offset(new_offset);
-        }
+        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) {
@@ -291,6 +282,7 @@ class FDC1004 {
             // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
             baseline_results[m.id.transaction_index]));
         if (m.id.transaction_index == 0) {
+            send_followup(m);
             return;
         }
         measurement += fdc1004_utils::convert_reads(baseline_results[0],
@@ -350,7 +342,6 @@ class FDC1004 {
     OwnQueue &own_queue;
     hardware::SensorHardwareBase &hardware;
 
-    ot_utils::filters::SimpleMovingAverage<int32_t> filter{};
     uint8_t sensor_binding{2};
     fdc1004::FDC1004RegisterMap _registers{};
     bool _initialized = false;
@@ -371,10 +362,49 @@ class FDC1004 {
     bool echoing = false;
     bool bind_sync = false;
     bool max_capacitance_sync = false;
-    std::bitset<2> data_stable{"00"};
     std::array<uint16_t, 2> baseline_results{};
     std::array<uint16_t, 2> polling_results{};
 
+    auto send_followup(i2c::messages::TransactionResponse &m) -> void {
+        auto address = utils::reg_from_id<uint8_t>(m.id.token);
+        uint8_t followup_address = 0;
+        auto tags = utils::tags_from_token(m.id.token);
+        i2c::messages::TransactionIdentifier id = m.id;
+        switch (address) {
+            case static_cast<uint8_t>(fdc1004::Registers::FDC_CONF):
+                // After CONF, read MSB for transaction index 0
+                followup_address =
+                    (measure_mode == fdc1004::MeasureConfigMode::ONE)
+                        ? static_cast<uint8_t>(fdc1004::Registers::MEAS1_MSB)
+                        : static_cast<uint8_t>(fdc1004::Registers::MEAS2_MSB);
+                id.transaction_index = 0;
+                break;
+            case static_cast<uint8_t>(fdc1004::Registers::MEAS1_MSB):
+                // After MSB, read LSB for index 1
+                followup_address =
+                    static_cast<uint8_t>(fdc1004::Registers::MEAS1_LSB);
+                id.transaction_index = 1;
+                break;
+            case static_cast<uint8_t>(fdc1004::Registers::MEAS2_MSB):
+                // After MSB, read LSB for index 1
+                followup_address =
+                    static_cast<uint8_t>(fdc1004::Registers::MEAS2_LSB);
+                id.transaction_index = 1;
+                break;
+            default:
+                // If this isn't one of the hardcoded regs, do nothing
+                return;
+        }
+
+        i2c::messages::MaxMessageBuffer buffer;
+        buffer[0] = followup_address;
+
+        // Overwrite the token to get the updated address
+        id.token = utils::build_id(fdc1004::ADDRESS, followup_address, tags);
+        // This maintains the entire transaction ID of the original request.
+        writer.transact(fdc1004::ADDRESS, 1, buffer, 2, id, own_queue);
+    }
+
     auto build_mode_1_configuration_register() -> fdc1004::ConfMeasure1 {
         _registers.config_measure_1.CHA =
             static_cast<uint8_t>(fdc1004::CHA::CIN1);
diff --git a/include/sensors/core/tasks/capacitive_sensor_task.hpp b/include/sensors/core/tasks/capacitive_sensor_task.hpp
@@ -44,8 +44,14 @@ class CapacitiveMessageHandler {
 
     void visit(i2c::messages::TransactionResponse &m) {
         auto reg_id = utils::reg_from_id<uint8_t>(m.id.token);
+        if (reg_id == static_cast<uint8_t>(fdc1004::Registers::FDC_CONF)) {
+            driver.handle_fdc_response(m);
+            return;
+        }
         if ((reg_id != static_cast<uint8_t>(fdc1004::Registers::MEAS1_MSB)) &&
-            (reg_id != static_cast<uint8_t>(fdc1004::Registers::MEAS2_MSB))) {
+            (reg_id != static_cast<uint8_t>(fdc1004::Registers::MEAS2_MSB)) &&
+            (reg_id != static_cast<uint8_t>(fdc1004::Registers::MEAS1_LSB)) &&
+            (reg_id != static_cast<uint8_t>(fdc1004::Registers::MEAS2_LSB))) {
             return;
         }
 
diff --git a/include/sensors/core/utils.hpp b/include/sensors/core/utils.hpp
@@ -70,6 +70,10 @@ template <std::ranges::range Tags>
     return bool(token & (1 << static_cast<size_t>(tag)));
 }
 
+[[nodiscard]] inline constexpr auto tags_from_token(uint32_t token) -> uint8_t {
+    return static_cast<uint8_t>(token & 0xff);
+}
+
 [[nodiscard]] inline constexpr auto build_id(uint16_t address, uint8_t reg,
                                              uint8_t tags = 0) -> uint32_t {
     return (static_cast<uint32_t>(address) << 16) |
diff --git a/include/sensors/simulation/fdc1004.hpp b/include/sensors/simulation/fdc1004.hpp
@@ -13,9 +13,12 @@ class FDC1004 : public I2CRegisterMap<uint8_t, uint16_t> {
         : I2CRegisterMap(
               fdc1004::ADDRESS,
               {{static_cast<uint8_t>(fdc1004::Registers::CONF_MEAS1), 0},
-               {static_cast<uint8_t>(fdc1004::Registers::FDC_CONF), 0},
+               // Mock out the Read Ready bits as set
+               {static_cast<uint8_t>(fdc1004::Registers::FDC_CONF), 0x000F},
                {static_cast<uint8_t>(fdc1004::Registers::MEAS1_MSB), 100},
                {static_cast<uint8_t>(fdc1004::Registers::MEAS1_LSB), 0},
+               {static_cast<uint8_t>(fdc1004::Registers::MEAS2_MSB), 100},
+               {static_cast<uint8_t>(fdc1004::Registers::MEAS2_LSB), 0},
                {static_cast<uint8_t>(fdc1004::Registers::DEVICE_ID),
                 fdc1004_utils::DEVICE_ID}}) {}
 };
diff --git a/sensors/tests/test_capacitive_sensor.cpp b/sensors/tests/test_capacitive_sensor.cpp
