From 54577fee431db28773aad689fe4610a26f5d81aa Mon Sep 17 00:00:00 2001 From: vegano1 Date: Wed, 11 Dec 2024 07:24:14 -0500 Subject: [PATCH] save --- .../flex-stacker/flex-stacker/tmf8821.hpp | 116 +++++++++++++----- .../flex-stacker/tmf8821_registers.hpp | 38 +++--- .../flex-stacker/tof_driver_task.hpp | 87 ++++++------- 3 files changed, 142 insertions(+), 99 deletions(-) diff --git a/stm32-modules/include/flex-stacker/flex-stacker/tmf8821.hpp b/stm32-modules/include/flex-stacker/flex-stacker/tmf8821.hpp index 6a975d20f..3f172dc33 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/tmf8821.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/tmf8821.hpp @@ -7,10 +7,8 @@ #include #include -#include "core/bit_utils.hpp" #include "flex-stacker/tmf8821_registers.hpp" #include "systemwide.h" -//#include "tof_driver_policy.hpp" namespace tmf8821 { using namespace std::numbers; @@ -22,48 +20,106 @@ concept TMF8821Policy = requires(P p, uint16_t dev_addr , uint16_t reg, uint16_t { p.i2c_write(dev_addr, reg, data, size) } -> std::same_as; }; +constexpr uint16_t TOF_DEFAULT_ADDRESS = 0x41 << 1; +constexpr uint16_t TOF_X_ADDRESS = 0x39 << 1; +constexpr uint16_t TOF_Z_ADDRESS = 0x40 << 1; + class TMF8821 { public: - template - auto initialize_sensor(const TMF8821RegisterMap& registers, Policy* policy, - TOFSensorID sensor_id) -> bool { + 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 { + if (!_policy) _policy = policy; + + // Check mode + // Do fw update Aif required + + if(!configure_sensor(registers, sensor_id)) return false; + + _initialized = _tof_x_init && _tof_z_init; return true; } - template - requires WritableRegister - auto write(Reg reg, Policy* policy, TOFSensorID sensor_id) -> bool { - // Ignore the typical linter warning because we're only using - // this on __packed structures that mimic hardware registers - // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) - //auto value = *reinterpret_cast(®); - //value &= Reg::value_mask; - auto value = 0; - auto dev_address = 0x41; // TODO: Fix - return policy.i2c_write(dev_address, Reg::address, value); + auto update_enable(const TMF8821RegisterMap& registers, TOFSensorID sensor_id) -> bool { + if (!_initialized) return false; + auto reg = registers.enable; + reg.padding_1 = 0; + return set_register(reg, sensor_id).has_value(); + } + + auto write(RegisterType type, uint16_t reg, uint32_t* data, TOFSensorID sensor_id) -> std::optional { + using RT = std::optional; + 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) -> std::optional { using RT = std::optional; - // TODO: This should be done by some message builder - auto size = 1; - auto dev_address = 0x41; // TODO: Fix - - static_cast(size); - static_cast(dev_address); - return RT(); - //auto ret = policy.i2c_read(dev_address, Reg::address, size); - //if (!ret.has_value()) { - // return RT(); - //} - // Ignore the typical linter warning because we're only using - // this on __packed structures that mimic hardware registers + auto dev_address = get_sensor_address(sensor_id); + auto [res, data] = _policy->i2c_read(dev_address, reg, 1); + if (res != 0) return RT(); + auto value = static_cast(*data.data()); + return RT(value); + } + + // 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; + return TOF_DEFAULT_ADDRESS; + } + + private: + + template + requires ReadableRegister + auto read_register(TOFSensorID sensor_id) -> std::optional { + using RT = std::optional; + 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) + return RT(*reinterpret_cast(&ret.value())); + } + + template + requires WritableRegister + auto set_register(Reg reg, TOFSensorID sensor_id) -> std::optional { + using RT = std::optional; + // TODO: Verify the Reg::mode (RegisterType) + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) - //return RT(*reinterpret_cast(&ret.value())); + auto value = *reinterpret_cast(®); + value &= Reg::value_mask; + auto ret = write(Reg::mode, Reg::address, &value, sensor_id); + if (!ret.has_value()) return RT(); + return RT(reg); + } + + 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}; bool _initialized = false; + bool _tof_x_init = false; + bool _tof_z_init = false; }; } // namespace tmf8821 diff --git a/stm32-modules/include/flex-stacker/flex-stacker/tmf8821_registers.hpp b/stm32-modules/include/flex-stacker/flex-stacker/tmf8821_registers.hpp index 0aa35ce4f..349dd4568 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/tmf8821_registers.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/tmf8821_registers.hpp @@ -24,7 +24,6 @@ #include namespace tmf8821 { -constexpr uint16_t DEFAULT_ADDRESS = 0x41 << 1; // Any appid, any cid_rid – Registers always available enum class BaseRegisters : uint8_t { @@ -237,18 +236,16 @@ inline auto is_valid_address(RegisterType type, const uint16_t reg) -> bool { return false; } -//auto value_to_register(RegisterType type, uint32_t reg) -> - /** Template concept to constrain what structures encapsulate registers.*/ template // Struct has a valid register address // Struct has an integer with the total number of bits in a register. // This is used to mask the value before writing it to the sensor. concept TMF8821Register = - std::same_as, - std::remove_cvref_t> && - std::integral && - std::is_array_v; + std::same_as, + std::remove_cvref_t> && + std::integral && + std::integral; template concept WritableRegister = requires() { @@ -261,45 +258,44 @@ concept ReadableRegister = requires() { }; struct __attribute__((packed, __may_alias__)) AppID { - // TODO: We might want to change this to uint32_t - static constexpr BaseRegisters address = BaseRegisters::APPID; + static constexpr auto mode = RegisterType::BASE; + static constexpr auto address = (uint16_t) BaseRegisters::APPID; static constexpr bool readable = true; static constexpr bool writable = false; static constexpr uint32_t value_mask = (1 << 8) - 1; - uint32_t C7 : 8 = 0; + uint8_t C7 : 8 = 0; }; - struct __attribute__((packed, __may_alias__)) Minor { - // TODO: We might want to change this to uint32_t - static constexpr BaseRegisters address = BaseRegisters::MINOR; + static constexpr auto mode = RegisterType::BASE; + static constexpr auto address = (uint16_t) BaseRegisters::MINOR; static constexpr bool readable = true; static constexpr bool writable = false; static constexpr uint32_t value_mask = (1 << 8) - 1; - uint32_t minor : 8 = 0; + uint8_t minor : 8 = 0; }; struct __attribute__((packed, __may_alias__)) Enable { - // TODO: We might want to change this to uint32_t - static constexpr BaseRegisters address = BaseRegisters::ENABLE; + static constexpr auto mode = RegisterType::BASE; + static constexpr auto address = (uint16_t) BaseRegisters::ENABLE; static constexpr bool readable = true; static constexpr bool writable = true; static constexpr uint32_t value_mask = (1 << 8) - 1; + // TODO: Do we need the size of the register? // Do these need to be uint32_t? - uint32_t pon : 1 = 1; - uint32_t padding_1 : 4 = 0; - uint32_t powerup_select : 2 = 0; - uint32_t cpu_ready : 1 = 0; + uint8_t pon : 1 = 1; + uint8_t padding_1 : 4 = 0; + uint8_t powerup_select : 2 = 0; + uint8_t cpu_ready : 1 = 0; }; struct TMF8821RegisterMap { Enable enable = {}; }; - // Registers are all 32 bits using RegisterSerializedType = uint32_t; // Type definition to allow type aliasing for pointer dereferencing diff --git a/stm32-modules/include/flex-stacker/flex-stacker/tof_driver_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/tof_driver_task.hpp index 9da89e8c5..8cf7ba927 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/tof_driver_task.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/tof_driver_task.hpp @@ -49,10 +49,6 @@ App Mode - add mechanism to set/get factory calibration */ -// This is the default address, this will be changed in sw. -static constexpr const uint8_t TOF_DEFAULT_ADDR = (0x41) - << 1; // 0x41 DEFAULT address - namespace tof_driver_task { using namespace tof::hardware; using namespace tmf8821; @@ -61,7 +57,13 @@ using Message = messages::TOFDriverMessage; static constexpr tmf8821::TMF8821RegisterMap tof_x_config{ // TODO: Change these defaults once available - .enable = {.pon = 1, .powerup_select = 0} + .enable = {.pon = 0, .powerup_select = 0} +}; + + +static constexpr tmf8821::TMF8821RegisterMap tof_z_config{ + // TODO: Change these defaults once available + .enable = {.pon = 0, .powerup_select = 0} }; template