add startup tmf8821

stm32-modules/flex-stacker/firmware/tof_sensor/tof_driver_policy.cpp

@@ -1,9 +1,11 @@
 #include <stdint.h>
 
+#include "FreeRTOS.h"
 #include "firmware/tof_driver_policy.hpp"
 #include "firmware/tof_sensor_hardware.h"
 #include "firmware/hardware_iface.hpp"
 #include "systemwide.h"
+#include "task.h"
 
 using namespace tof::hardware;
 
@@ -20,3 +22,7 @@ auto TOFDriverPolicy::i2c_read(uint16_t dev_addr, uint16_t reg, uint16_t size)
     -> i2c::hardware::RxTxReturn {
     return i2c_comms->i2c_read(dev_addr, reg, size);
 }
+
+auto TOFDriverPolicy::sleep_ms(uint32_t ms) -> void {
+    vTaskDelay(pdMS_TO_TICKS(ms));
+}

stm32-modules/include/flex-stacker/firmware/tof_driver_policy.hpp

@@ -24,6 +24,7 @@ class TOFDriverPolicy {
     auto i2c_write(uint16_t dev_addr, uint16_t reg, uint8_t *data,
                    uint16_t size) -> RxTxReturn;
     auto enable_tof_sensor(TOFSensorID sensor_id, bool enable) -> void;
+    auto sleep_ms(uint32_t ms) -> void;
 
   private:
     I2CBase *i2c_comms{nullptr};

stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp

@@ -12,6 +12,7 @@
 #include "core/ack_cache.hpp"
 #include "core/gcode_parser.hpp"
 #include "core/version.hpp"
+#include "errors.hpp"
 #include "flex-stacker/errors.hpp"
 #include "flex-stacker/gcodes.hpp"
 #include "flex-stacker/messages.hpp"
@@ -1202,7 +1203,7 @@ class HostCommsTask {
             .id = id, .sensor_id = gcode.sensor_id, .enable = gcode.enable};
         if (!task_registry->send(message, TICKS_TO_WAIT_ON_SEND)) {
             auto wrote_to = errors::write_into(
-                tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
+                tx_into, tx_limit, errors::ErrorCode::GCODE_CACHE_FULL);
             ack_only_cache.remove_if_present(id);
             return std::make_pair(false, wrote_to);
         }

stm32-modules/include/flex-stacker/flex-stacker/tmf8821.hpp

@@ -27,23 +27,41 @@ constexpr uint16_t TOF_Z_ADDRESS = 0x40 << 1;
 class TMF8821 {
   public:
     TMF8821(TOFDriverPolicy* policy) : _policy(policy) {}
-    TMF8821(const TMF8821& c) = delete;
-    TMF8821(const TMF8821&& c) = delete;
-    auto operator=(const TMF8821& c) = delete;
-    auto operator=(const TMF8821&& c) = delete;
-    ~TMF8821() = default;
 
     auto initialize_sensor(const TMF8821RegisterMap& registers,
                            TOFDriverPolicy* policy, TOFSensorID sensor_id) -> bool {
+
+        // FOR TESTING
+        return true;
+
+        if (_initialized) return true;
         if (!_policy) _policy = policy;
+        _registers = registers;
+        _sensor_id = sensor_id;
+
+        _policy->enable_tof_sensor(sensor_id, true);
+        _policy->sleep_ms(20);  // sleep for 20ms for device to boot
 
-        // Check mode
-        // Do fw update Aif required
+        // Make sure the sensor is ready
+        if (!set_sensor_ready(sensor_id)) return false;
+        // Make sure the sensor is not in bootloader mode
+        if (get_sensor_mode(sensor_id) == TOFSensorMode::BOOTLOADER) {
+            if (!handle_bootloader(sensor_id)) return false;
+            // Update was successful, configure the sensor
+        }
 
         if(!configure_sensor(registers, sensor_id)) return false;
 
-        _initialized = _tof_x_init && _tof_z_init;
-        return true;
+        _initialized = true;
+        return _initialized;
+    }
+
+    [[nodiscard]] auto get_enable_reg() -> std::optional<tmf8821::Enable> {
+        auto ret = read_register<tmf8821::Enable>(_sensor_id);
+        if (ret.has_value()) {
+            _registers.enable = ret.value();
+        }
+        return ret;
     }
 
     auto update_enable(const TMF8821RegisterMap& registers, TOFSensorID sensor_id) -> bool {
@@ -53,18 +71,19 @@ class TMF8821 {
         return set_register(reg, sensor_id).has_value();
     }
 
-    auto write(RegisterType type, uint16_t reg, uint32_t* data, TOFSensorID sensor_id) -> std::optional<RegisterSerializedType> {
+    auto write(uint16_t reg, uint32_t* data, TOFSensorID sensor_id) -> std::optional<RegisterSerializedType> {
         using RT = std::optional<RegisterSerializedType>;
+        // TODO: validate register based on the mode
         auto dev_address = get_sensor_address(sensor_id);
-        // TODO: (uint8_t *) should be uint32_t 
         auto [res, _] = _policy->i2c_write(dev_address, reg, (uint8_t *) data, 1);
         if (res != 0) return RT();
         return RT(res);
     }
 
-    auto read(RegisterType type, uint32_t reg, TOFSensorID sensor_id)
+    auto read(uint32_t reg, TOFSensorID sensor_id)
         -> std::optional<RegisterSerializedType> {
         using RT = std::optional<RegisterSerializedType>;
+        // TODO: validate register based on the mode
         auto dev_address = get_sensor_address(sensor_id);
         auto [res, data] = _policy->i2c_read(dev_address, reg, 1);
         if (res != 0) return RT();
@@ -74,7 +93,6 @@ class TMF8821 {
 
     // Gets the sensor i2c address
     auto get_sensor_address(TOFSensorID sensor_id) -> uint16_t {
-        // TODO: This probably needs account for the individual sensors
         if (!_initialized) return TOF_DEFAULT_ADDRESS;
         if (sensor_id == TOF_X) return TOF_X_ADDRESS;
         if (sensor_id == TOF_Z) return TOF_Z_ADDRESS;
@@ -87,7 +105,6 @@ class TMF8821 {
     requires ReadableRegister<Reg>
     auto read_register(TOFSensorID sensor_id) -> std::optional<Reg> {
         using RT = std::optional<Reg>;
-        auto type = Reg::type;  // TODO: use type based on mode
         auto ret = read(Reg::address, sensor_id);
         if (!ret.has_value()) return RT();
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
@@ -103,23 +120,57 @@ class TMF8821 {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
         auto value = *reinterpret_cast<RegisterSerializedTypeA*>(&reg);
         value &= Reg::value_mask;
-        auto ret = write(Reg::mode, Reg::address, &value, sensor_id);
+        auto ret = write(Reg::address, &value, sensor_id);
         if (!ret.has_value()) return RT();
         return RT(reg);
     }
 
+    auto set_sensor_ready(TOFSensorID sensor_id) -> bool {
+        auto ret = read_register<tmf8821::Enable>(sensor_id);
+        if (!ret.has_value()) return false;
+
+        auto reg = static_cast<tmf8821::Enable>(ret.value());
+        if (!reg.pon || !reg.cpu_ready) {
+            _registers.enable.pon = 1;
+            _registers.enable.powerup_select = reg.powerup_select;
+            update_enable(_registers, sensor_id);
+            // check enable register again after 100ms
+            _policy->sleep_ms(100);
+            ret = read_register<tmf8821::Enable>(sensor_id);
+            if (ret.has_value()) {
+                auto reg = static_cast<tmf8821::Enable>(ret.value());
+                // device is not ready for comms
+                if (!reg.pon || !reg.cpu_ready) return false;
+            }
+        }
+        return true;
+    }
+
+    auto get_sensor_mode(TOFSensorID sensor_id) -> TOFSensorMode {
+        // check what app the sensor is running
+        auto ret = read_register<tmf8821::AppID>(sensor_id);
+        if (!ret.has_value()) return TOFSensorMode::UNKNOWN;
+        auto appid = static_cast<tmf8821::AppID>(ret.value()).appid;
+        _mode = static_cast<TOFSensorMode>(appid);
+        return _mode;
+    }
+
+    auto handle_bootloader(TOFSensorID sensor_id) -> bool {
+        // TODO: perform image download
+        return true;
+    }
+
     auto configure_sensor(const TMF8821RegisterMap& registers, TOFSensorID sensor_id) -> bool {
         if(!update_enable(registers, sensor_id)) return false;
 
-        if(sensor_id == TOF_X) _tof_x_init = true;
-        if(sensor_id == TOF_Z) _tof_z_init = true;
         return true;
     }
 
     TOFDriverPolicy* _policy{nullptr};
+    tmf8821::TMF8821RegisterMap _registers = {};
     bool _initialized = false;
-    bool _tof_x_init = false;
-    bool _tof_z_init = false;
+    TOFSensorMode _mode = TOFSensorMode::UNKNOWN;
+    TOFSensorID _sensor_id = TOFSensorID::NONE;
 };
 
 }  // namespace tmf8821

stm32-modules/include/flex-stacker/flex-stacker/tmf8821_registers.hpp

@@ -264,7 +264,7 @@ struct __attribute__((packed, __may_alias__)) AppID {
     static constexpr bool writable = false;
     static constexpr uint32_t value_mask = (1 << 8) - 1;
 
-    uint8_t C7 : 8 = 0;
+    uint8_t appid : 8 = 0;
 };
 
 struct __attribute__((packed, __may_alias__)) Minor {

stm32-modules/include/flex-stacker/flex-stacker/tof_driver_task.hpp

@@ -99,9 +99,12 @@ class TOFDriverTask {
 
         if (!_initialized) {
             _policy = policy;
-            _tmf8821.initialize_sensor(_tof_x_config, _policy, TOF_X);
-            _tmf8821.initialize_sensor(_tof_z_config, _policy, TOF_Z);
-            _initialized = true;
+            auto ret_x = _tmf8821_x.initialize_sensor(_tof_x_config, _policy, TOF_X);
+            if (!ret_x) _policy->enable_tof_sensor(TOF_X, false);
+
+            auto ret_z = _tmf8821_z.initialize_sensor(_tof_z_config, _policy, TOF_Z);
+            if (!ret_z) _policy->enable_tof_sensor(TOF_Z, false);
+            _initialized = ret_x && ret_z;
         }
 
         auto message = Message(std::monostate());
@@ -120,16 +123,8 @@ class TOFDriverTask {
 
     auto visit_message(const messages::GetTOFRegisterMessage& m) -> void {
         messages::HostCommsMessage response;
-        // TODO: Validate type, reg, and value
-        // TODO: type needs to be set based on the mode
-        auto type = RegisterType::BASE;
-        //auto data = _tmf8821.read(type, m.reg, m.sensor_id);
-
-        // FOR TESTING i2c write
-        auto value = static_cast<uint32_t>(0x51);  // 81d
-        auto data = _tmf8821.write(type, m.reg, &value, m.sensor_id);
-        // FOR TESTING
-
+        auto tmf8821 = get_driver(m.sensor_id);
+        auto data = tmf8821.read(m.reg, m.sensor_id);
         if (!data.has_value()) {
           response = messages::ErrorMessage{
                 .code = errors::ErrorCode::TMC2160_READ_ERROR};
@@ -147,11 +142,10 @@ class TOFDriverTask {
 
     auto visit_message(const messages::SetTOFRegisterMessage& m) -> void {
         auto response = messages::AcknowledgePrevious{.responding_to_id = m.id};
-        // TODO: Validate type, reg, and value
-        auto type = RegisterType::BASE;
         auto reg = static_cast<uint16_t>(m.reg);
         auto value = static_cast<uint32_t>(m.data);
-        auto data = _tmf8821.write(type, reg, &value, m.sensor_id);
+        auto tmf8821 = get_driver(m.sensor_id);
+        auto data = tmf8821.write(reg, &value, m.sensor_id);
         if (!data.has_value()) {
             response.with_error = errors::ErrorCode::TMC2160_WRITE_ERROR;
         }
@@ -167,12 +161,19 @@ class TOFDriverTask {
             response, Queues::HostCommsAddress));
     }
 
+    auto get_driver(TOFSensorID sensor_id) -> tmf8821::TMF8821 {
+        if (sensor_id == TOF_X) return _tmf8821_x;
+        if (sensor_id == TOF_Z) return _tmf8821_z;
+        return _tmf8821_x;
+    }
+
     Queue& _message_queue;
     Aggregator* _task_registry;
     TOFDriverPolicy* _policy;
     bool _initialized = false;
 
-    tmf8821::TMF8821 _tmf8821{nullptr};
+    tmf8821::TMF8821 _tmf8821_x{nullptr};
+    tmf8821::TMF8821 _tmf8821_z{nullptr};
     tmf8821::TMF8821RegisterMap _tof_x_config = tof_x_config;
     tmf8821::TMF8821RegisterMap _tof_z_config = tof_z_config;
 };

stm32-modules/include/flex-stacker/systemwide.h

@@ -8,10 +8,17 @@ typedef enum MotorID {
 } MotorID;
 
 typedef enum TOFSensorID {
+    NONE,
     TOF_Z,
     TOF_X,
 } TOFSensorID;
 
+typedef enum TOFSensorMode {
+    UNKNOWN    = 0x00,
+    MEASURE    = 0x03,
+    BOOTLOADER = 0x80,
+} TOFSensorMode;
+
 typedef enum StatusBarID {
     Internal = 0,
     External,