5#include <proto/main.pb.h>
6#include "block/mblock_mdr.h"
8#include "entity/visitor.h"
10bool blocks::MMDRBlock::write_to_hardware() {
11 TRY(write_calibration_to_hardware());
12 TRY(write_cell_modes_to_hardware());
13 return UnitResult::ok();
16UnitResult blocks::MMDRBlock::write_calibration_to_hardware() {
17 for (
size_t i = 0; i < NUM_CELLS; i++) {
18 if (!hardware->write_calibration_input_offsets(i, calibration[i].offset_x, calibration[i].offset_y))
19 return UnitResult::err_fmt(
"MMDRBlock::calibration for cell %d values offset_x, offset_y not accepted",
21 if (!hardware->write_calibration_output_offset(i, calibration[i].offset_z))
22 return UnitResult::err_fmt(
"MMDRBlock::calibration for cell %d values offset_z not accepted", i);
24 return UnitResult::ok();
27ConfigResult blocks::MMDRBlock::config(
const pb_Item &item) {
28#ifdef ANABRID_DEBUG_ENTITY_CONFIG
29 Serial.println(__PRETTY_FUNCTION__);
32 if (item.which_kind == pb_Item_mul_config_tag) {
33 cell_modes.fill(CellMode::MUL);
34 return ConfigResult::ok(
true);
37 if(item.which_kind != pb_Item_mdr_config_tag)
38 return ConfigResult::ok(
false);
40 auto& mdr_config = item.kind.mdr_config;
43 if (mdr_config.operations_count > NUM_CELLS)
44 return ConfigResult::err(
"MDRConfig: too many operations");
46 for (
size_t idx = 0; idx < mdr_config.operations_count; ++idx) {
47 auto op =
static_cast<uint8_t
>(mdr_config.operations[idx]);
48 if (op >= std::size(enum_to_mode))
49 return ConfigResult::err_fmt(
"MDRConfig: unknown operation %d at index %d", op, idx);
52 auto mode = enum_to_mode[op];
53 if (
mode != CellMode::MUL &&
mode != CellMode::SQUARE &&
54 mode != CellMode::DIV &&
mode != CellMode::ROOT &&
56 return ConfigResult::err_fmt(
"MDRConfig: operation %d maps to unsupported cell mode at index %d", op, idx);
62 this->reset(entities::ResetAction::CIRCUIT_RESET);
63 this->reset(entities::ResetAction::OVERLOAD_RESET);
64 this->hardware->write_modes(cell_modes[0], cell_modes[1], cell_modes[2],
68 for (
size_t idx = 0; idx < mdr_config.operations_count; ++idx)
69 set_cell_mode(idx, enum_to_mode[
static_cast<uint8_t
>(mdr_config.operations[idx])]);
71 return ConfigResult::ok(
true);
74void blocks::MMDRBlock::extract(entities::ExtractVisitor &collector) {
75 Entity::extract(collector);
77 auto& config = collector.create(pb_Item_mdr_config_tag);
78 auto& mdr_config = config.kind.mdr_config;
80 mdr_config.operations_count = 0;
81 if (collector.include_configuration()) {
82 for (
int i = 0; i < MMDRBlock::NUM_CELLS; i++) {
83 mdr_config.operations[mdr_config.operations_count++] =
84 mode_to_enum[
static_cast<uint8_t
>(get_cell_modes()[i])];
88 mdr_config.calibration_count = 0;
89 if (collector.include_calibration()) {
90 for (
int i = 0; i < MMDRBlock::NUM_CELLS; i++) {
91 mdr_config.calibration[mdr_config.calibration_count++] = {
92 .offset_x = calibration[i].offset_x,
93 .offset_y = calibration[i].offset_y,
94 .offset_z = calibration[i].offset_z,
100blocks::MMDRBlock::MMDRBlock(SLOT slot, blocks::MMDRBlockHAL *hardware)
101 : MBlock(slot, hardware), hardware(hardware) {
102 classifier.type =
static_cast<uint8_t
>(TYPE);
105bool blocks::MMDRBlock::init() {
107 return FunctionBlock::init() && hardware->init();
110void blocks::MMDRBlock::reset(entities::ResetAction action) {
111 FunctionBlock::reset(action);
113 if (action.has(entities::ResetAction::CIRCUIT_RESET)) {
114 for (
auto idx = 0u; idx < MMDRBlock::NUM_CELLS; idx++)
115 cell_modes[idx] = CellMode::MUL;
118 if (action.has(entities::ResetAction::CALIBRATION_RESET)) {
119 for (
auto idx = 0u; idx < MMDRBlock::NUM_CELLS; idx++)
120 calibration[idx] = {};
123 if (action.has(entities::ResetAction::OVERLOAD_RESET)) {
124 hardware->reset_overload_flags();
128const std::array<blocks::CellCalibration, blocks::MMDRBlock::NUM_CELLS> &
129blocks::MMDRBlock::get_calibration()
const {
133blocks::CellCalibration blocks::MMDRBlock::get_calibration(uint8_t cell_idx)
const {
134 if (cell_idx >= NUM_CELLS)
136 return calibration[cell_idx];
139const std::array<blocks::MMDRBlock::CellMode, blocks::MMDRBlock::NUM_CELLS> &
140blocks::MMDRBlock::get_cell_modes()
const {
144blocks::MMDRBlock::CellMode blocks::MMDRBlock::get_cell_modes(uint8_t cell_idx)
const {
145 if (cell_idx >= NUM_CELLS)
147 return cell_modes[cell_idx];
150void blocks::MMDRBlock::set_cell_modes(std::array<CellMode, NUM_CELLS> _cell_modes) {
151 cell_modes = _cell_modes;
154void blocks::MMDRBlock::set_cell_mode(uint8_t mul_idx, CellMode cell_mode) {
155 if (mul_idx >= NUM_CELLS)
157 cell_modes[mul_idx] = cell_mode;
160UnitResult blocks::MMDRBlock::write_cell_modes_to_hardware() {
161 if (!hardware->write_modes(cell_modes[0], cell_modes[1], cell_modes[2], cell_modes[3]))
162 return UnitResult::err(
"Error writing cell modes to hardware");
163 return UnitResult::ok();