Skip to content

Commit

Permalink
save
Browse files Browse the repository at this point in the history
  • Loading branch information
@vegano1
vegano1 committed Dec 11, 2024
1 parent 42d5617 commit 54577fe
Show file tree
Hide file tree
Showing 3 changed files with 142 additions and 99 deletions.
116 changes: 86 additions & 30 deletions stm32-modules/include/flex-stacker/flex-stacker/tmf8821.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,8 @@
#include <numbers>
#include <optional>

#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;
Expand All @@ -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<RxTxReturn>;
};

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 <TMF8821Policy Policy>
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 <tmf8821::TMF8821Register Reg, TMF8821Policy Policy>
requires WritableRegister<Reg>
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<RegisterSerializedTypeA*>(&reg);
//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<RegisterSerializedType> {
using RT = std::optional<RegisterSerializedType>;
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<RegisterSerializedType> {
using RT = std::optional<RegisterSerializedType>;
// TODO: This should be done by some message builder
auto size = 1;
auto dev_address = 0x41; // TODO: Fix

static_cast<void>(size);
static_cast<void>(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<uint32_t>(*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 <tmf8821::TMF8821Register Reg>
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)
return RT(*reinterpret_cast<Reg*>(&ret.value()));
}

template <tmf8821::TMF8821Register Reg>
requires WritableRegister<Reg>
auto set_register(Reg reg, TOFSensorID sensor_id) -> std::optional<Reg> {
using RT = std::optional<Reg>;
// TODO: Verify the Reg::mode (RegisterType)

// NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
//return RT(*reinterpret_cast<Reg*>(&ret.value()));
auto value = *reinterpret_cast<RegisterSerializedTypeA*>(&reg);
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
38 changes: 17 additions & 21 deletions stm32-modules/include/flex-stacker/flex-stacker/tmf8821_registers.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,6 @@
#include <cstdint>

namespace tmf8821 {
constexpr uint16_t DEFAULT_ADDRESS = 0x41 << 1;

// Any appid, any cid_rid – Registers always available
enum class BaseRegisters : uint8_t {
Expand Down Expand Up @@ -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 <typename Reg>
// 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<decltype(Reg::address)>,
std::remove_cvref_t<Registers&>> &&
std::integral<decltype(Reg::value_mask)> &&
std::is_array_v<decltype(Reg::mode_map)>;
std::same_as<std::remove_cvref_t<decltype(Reg::mode)>,
std::remove_cvref_t<RegisterType&>> &&
std::integral<decltype(Reg::address)> &&
std::integral<decltype(Reg::value_mask)>;

template <typename Reg>
concept WritableRegister = requires() {
Expand All @@ -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
Expand Down
87 changes: 39 additions & 48 deletions stm32-modules/include/flex-stacker/flex-stacker/tof_driver_task.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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 <template <class> class QueueImpl>
Expand Down Expand Up @@ -97,10 +99,8 @@ class TOFDriverTask {

if (!_initialized) {
_policy = policy;
auto s1 = _tmf8821.initialize_sensor(_tof_x_config, _policy, TOF_X);
static_cast<void>(s1);
//auto s2 = _tmf8821.initialize(_policy, TOF_Z);
// sensor_status = SensorStatus(ret1, ret2)
_tmf8821.initialize_sensor(_tof_x_config, _policy, TOF_X);
_tmf8821.initialize_sensor(_tof_z_config, _policy, TOF_Z);
_initialized = true;
}

Expand All @@ -120,49 +120,39 @@ class TOFDriverTask {

auto visit_message(const messages::GetTOFRegisterMessage& m) -> void {
messages::HostCommsMessage response;

// TODO: This needs to be set based on the mode
// TODO: Validate type, reg, and value
// TODO: type needs to be set based on the mode
auto type = RegisterType::BASE;
auto reg = static_cast<uint32_t>(BaseRegisters::ENABLE);
auto data = _tmf8821.read(type, reg, m.sensor_id);
//auto data = _tmf8821.write(reg, value, m.sensor_id);

static_cast<void>(data);
return;


//auto reg = static_cast<uint16_t>(m.reg);
//auto size = 1;

//auto [res, data] = _policy->i2c_read(TOF_DEFAULT_ADDR, reg, size);
//if (res != 0) {
// response = messages::ErrorMessage{
// .code = errors::ErrorCode::TMC2160_READ_ERROR};
//} else {
// auto value = static_cast<uint32_t>(*data.data());
// response = messages::GetTOFRegisterResponse{
// .responding_to_id = m.id,
// .sensor_id = m.sensor_id,
// .reg = res,
// .data = value,
// };
//}
//static_cast<void>(_task_registry->send_to_address(
// response, Queues::HostCommsAddress));
//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

if (!data.has_value()) {
response = messages::ErrorMessage{
.code = errors::ErrorCode::TMC2160_READ_ERROR};
} else {
response = messages::GetTOFRegisterResponse{
.responding_to_id = m.id,
.sensor_id = m.sensor_id,
.reg = m.reg,
.data = data.value(),
};
}
static_cast<void>(_task_registry->send_to_address(
response, Queues::HostCommsAddress));
}

auto visit_message(const messages::SetTOFRegisterMessage& m) -> void {
// TODO: This should be done by some message builder
auto reg = static_cast<uint16_t>(m.reg);
auto value = static_cast<uint8_t>(m.data);
auto size = 1;

auto response = messages::AcknowledgePrevious{.responding_to_id = m.id};
// TODO: Validate register and value

auto [res, data] =
_policy->i2c_write(TOF_DEFAULT_ADDR, reg, &value, size);
if (res != 0) {
// 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);
if (!data.has_value()) {
response.with_error = errors::ErrorCode::TMC2160_WRITE_ERROR;
}
static_cast<void>(_task_registry->send_to_address(
Expand All @@ -182,7 +172,8 @@ class TOFDriverTask {
TOFDriverPolicy* _policy;
bool _initialized = false;

tmf8821::TMF8821 _tmf8821{};
tmf8821::TMF8821 _tmf8821{nullptr};
tmf8821::TMF8821RegisterMap _tof_x_config = tof_x_config;
tmf8821::TMF8821RegisterMap _tof_z_config = tof_z_config;
};
}; // namespace tof_driver_task

0 comments on commit 54577fe

Please sign in to comment.