diff --git a/docs/shield_position.md b/docs/shield_position.md new file mode 100644 index 00000000..9ebaf1f5 --- /dev/null +++ b/docs/shield_position.md @@ -0,0 +1,378 @@ +!!! note "" + Shield position is an API that allows to access position sensors connected through a Spin shield. + It provides a single entry point, ``shield.position``, for Hall effect sensors, incremental encoders, and Sin/Cos encoders. + Whatever the sensor type, the API can provide the same outputs: mechanical angle, electrical angle, mechanical speed, and electrical speed. + +## Include + +!!! note + ```cpp + #include + ``` + Make sure that Shield API is included to use ``shield.position``. + +## Initialization sequence + +For any position sensor, the recommended sequence is: + +1. Select the sensor with ``shield.position.init(...)`` +2. Adjust parameters if needed +3. Call ``shield.position.update(sample_time)`` periodically +4. Read angle and speed values + +!!! example + + ```cpp + shield.position.init(ABZ); + shield.position.setCountsPerRevolution(4096); + shield.position.setPolePairs(4); + + shield.position.update(100e-6F); + + float32_t electrical_angle = shield.position.getElectricalAngle(); + float32_t electrical_speed = shield.position.getElectricalSpeed(); + ``` + +## Common outputs + +Once initialized, all supported sensor types provide the same estimated outputs: + +- ``shield.position.getMechanicalAngle()`` +- ``shield.position.getElectricalAngle()`` +- ``shield.position.getMechanicalSpeed()`` +- ``shield.position.getElectricalSpeed()`` +- ``shield.position.getMechanicalPulsation()`` +- ``shield.position.getElectricalPulsation()`` + +## Common configuration functions + +The following functions can be used regardless of the selected sensor: + +- ``shield.position.setDirectionSign(...)`` +- ``shield.position.setPolePairs(...)`` +- ``shield.position.setElectricalOffset(...)`` + +These parameters are typically used to adapt the sensor to the motor and control convention: + +- reverse the positive rotation direction +- declare the number of motor pole pairs +- align the electrical zero used by the control + +`direction-sign` must be either `-1` or `1`, and `pole-pairs` must be strictly positive. + +For Hall sensors: + +- ``HALL_INTERPOLATION_NONE`` keeps a discrete Hall-based estimator and does not start the PLL +- with ``HALL_INTERPOLATION_NONE``, the speed output is only updated on Hall transitions, so it does not provide a continuous speed estimate +- ``HALL_INTERPOLATION_LINEAR`` enables the PLL-based interpolation to smooth angle and speed between Hall transitions +- the Hall mechanical angle is derived from Hall sectors and pole pairs, so it is not shifted by ``electrical-offset`` + +## Hall effect sensor + +Use the Hall sensor mode when your motor provides 3 digital Hall signals. + +### Minimal initialization + +1. Initialize the Hall sensor: + +```cpp +shield.position.init(HALL); +``` + +2. Configure the Hall-related parameters if needed: + +```cpp +uint8_t hall_table[6] = {5, 1, 0, 3, 4, 2}; + +shield.position.setPolePairs(4); +shield.position.setElectricalOffset(0.0F); +shield.position.setHallSectorTable(hall_table); +shield.position.setHallInterpolation(HALL_INTERPOLATION_LINEAR); +``` + +3. Update the estimator in your control loop: + +```cpp +shield.position.update(100e-6F); +``` + +4. Read the outputs: + +```cpp +float32_t mechanical_angle = shield.position.getMechanicalAngle(); +float32_t electrical_angle = shield.position.getElectricalAngle(); +float32_t electrical_speed = shield.position.getElectricalSpeed(); +``` + +### Raw Hall access + +If needed, the raw Hall state remains accessible: + +```cpp +uint8_t hall_state = shield.position.getHallState(); +``` + +Hall sensors only provide electrical-sector information, so ``getMechanicalAngle()`` is a mechanical angle modulo one electrical revolution divided by the number of pole pairs. + +### Typical Hall parameters + +The Hall effect sensor typically uses these parameters: + +- ``direction-sign`` +- ``pole-pairs`` +- ``electrical-offset`` +- ``hall-sector-table`` +- ``hall-interpolation`` + +The default Hall sequence used by OwnVerter is: + +- ``001 -> 011 -> 010 -> 110 -> 100 -> 101`` + +which corresponds to the default sector table: + +```cpp +{5, 1, 0, 3, 4, 2} +``` + +### Example + +```cpp +shield.position.init(HALL); + +uint8_t hall_table[6] = {5, 1, 0, 3, 4, 2}; + +shield.position.setDirectionSign(1); +shield.position.setPolePairs(4); +shield.position.setElectricalOffset(0.0F); +shield.position.setHallSectorTable(hall_table); +shield.position.setHallInterpolation(HALL_INTERPOLATION_LINEAR); + +shield.position.update(100e-6F); + +float32_t angle = shield.position.getElectricalAngle(); +float32_t speed = shield.position.getElectricalSpeed(); +``` + +## Incremental encoder + +Use the incremental encoder mode when your motor provides AB signals, with the timer peripheral handling the encoder interface wiring. + +### Minimal initialization + +1. Initialize the encoder: + +```cpp +shield.position.init(ABZ); +``` + +2. Configure the encoder parameters: + +```cpp +shield.position.setCountsPerRevolution(4096); +shield.position.setDirectionSign(1); +shield.position.setPolePairs(4); +shield.position.setElectricalOffset(0.0F); +``` + +3. Update the estimator: + +```cpp +shield.position.update(100e-6F); +``` + +4. Read the outputs: + +```cpp +float32_t mechanical_angle = shield.position.getMechanicalAngle(); +float32_t electrical_angle = shield.position.getElectricalAngle(); +float32_t mechanical_speed = shield.position.getMechanicalSpeed(); +``` + +### Raw encoder access + +If needed, the raw encoder count remains accessible: + +```cpp +uint32_t encoder_count = shield.position.getIncrementalEncoderValue(); +``` + +### Typical incremental encoder parameters + +The incremental encoder typically uses these parameters: + +- ``counts-per-revolution`` +- ``direction-sign`` +- ``pole-pairs`` +- ``electrical-offset`` + +### Example + +```cpp +shield.position.init(ABZ); + +shield.position.setCountsPerRevolution(4096); +shield.position.setDirectionSign(1); +shield.position.setPolePairs(4); +shield.position.setElectricalOffset(0.0F); + +shield.position.update(100e-6F); + +float32_t angle = shield.position.getElectricalAngle(); +float32_t speed = shield.position.getElectricalSpeed(); +``` + +## Sin/Cos encoder + +Use the Sin/Cos mode when your position sensor provides 2 analog channels: one sine and one cosine. + +!!! warning + ``shield.position.init(SINCOS)`` enables the semantic shield sensors, but converted Sin/Cos samples are only available once the ADC acquisition path has been started. Call the relevant data acquisition start sequence before relying on ``shield.position.update(...)``. + +### Minimal initialization + +1. Initialize the Sin/Cos encoder: + +```cpp +shield.position.init(SINCOS); +``` + +2. Configure the common motor parameters: + +```cpp +shield.position.setDirectionSign(1); +shield.position.setPolePairs(4); +shield.position.setElectricalOffset(0.0F); +``` + +3. Update the estimator: + +```cpp +shield.position.update(100e-6F); +``` + +4. Read the outputs: + +```cpp +float32_t mechanical_angle = shield.position.getMechanicalAngle(); +float32_t electrical_angle = shield.position.getElectricalAngle(); +float32_t electrical_speed = shield.position.getElectricalSpeed(); +``` + +### Raw Sin/Cos access + +If needed, the raw analog channels remain accessible: + +```cpp +float32_t sin_value = shield.position.getSinValue(); +float32_t cos_value = shield.position.getCosValue(); +``` + +### Typical Sin/Cos parameters + +The Sin/Cos encoder typically uses these parameters: + +- ``direction-sign`` +- ``pole-pairs`` +- ``electrical-offset`` + +### Example + +```cpp +shield.position.init(SINCOS); + +shield.position.setDirectionSign(1); +shield.position.setPolePairs(4); +shield.position.setElectricalOffset(0.0F); + +shield.position.update(100e-6F); + +float32_t angle = shield.position.getElectricalAngle(); +float32_t speed = shield.position.getElectricalSpeed(); +``` + +## Using a default sensor + +If your application always uses the same position sensor, you can define a default one in ``src/app.overlay``: + +```dts +/ { + chosen { + owntech,position-sensor = &abz; + }; +}; +``` + +Then initialize it in code with: + +```cpp +shield.position.initDefault(); +``` + +## Overriding default hardware parameters + +Most users can configure the sensor directly from C++ with the runtime setters shown above. + +If you want to change the default values stored by the shield, create or edit ``src/app.overlay``. + +### Example for a Hall application + +```dts +&default_motor { + pole-pairs = <4>; + direction-sign = <1>; + electrical-offset = <0x00000000>; + hall-sector-table = <5 1 0 3 4 2>; + hall-interpolation = "LINEAR"; +}; +``` + +### Example for an incremental encoder application + +```dts +&default_motor { + counts-per-revolution = <4096>; + pole-pairs = <4>; + direction-sign = <1>; + electrical-offset = <0x00000000>; +}; +``` + +### Example for a Sin/Cos application + +```dts +&default_motor { + pole-pairs = <4>; + direction-sign = <1>; + electrical-offset = <0x3dcccccd>; +}; +``` + +## Choosing between the 3 sensor types + +### Hall effect sensor + +Use Hall effect sensor when: + +- you need a simple digital position feedback +- you want a robust and low-cost solution +- moderate angle precision is enough + +### Incremental encoder + +Use an incremental encoder when: + +- you need high angular resolution +- you want precise speed estimation +- your sensor provides AB signals + +### Sin/Cos encoder + +Use a Sin/Cos encoder when: + +- you have 2 analog sensor channels available +- you want continuous analog position information +- you want the API to estimate angle from the sine and cosine channels + +## API Reference +::: doxy.powerAPI.class +name: PositionAPI diff --git a/zephyr/boards/shields/ownverter/ownverter_v0_9_0.overlay b/zephyr/boards/shields/ownverter/ownverter_v0_9_0.overlay index f2faf812..cd12b386 100644 --- a/zephyr/boards/shields/ownverter/ownverter_v0_9_0.overlay +++ b/zephyr/boards/shields/ownverter/ownverter_v0_9_0.overlay @@ -15,6 +15,17 @@ shield-version = "v0.9.0"; }; + default_motor: default-motor { + /* + * Placeholder motor configuration. + * Applications can override these values from app.overlay without + * changing the shield description. + */ + compatible = "motor-config"; + pole-pairs = <1>; + status = "okay"; + }; + powershield: power-shield{ compatible = "power-leg"; default-frequency = <200000>; @@ -374,6 +385,55 @@ status = "disabled"; }; }; + + shield-position-sensors { + hall: hall-sensor { + compatible = "shield-position-hall"; + position-sensor-name = "HALL"; + motor = <&default_motor>; + direction-sign = <1>; + electrical-offset = <0x00000000>; + hall-a-gpios = <&spin_header 7 GPIO_ACTIVE_HIGH>; + hall-b-gpios = <&spin_header 9 GPIO_ACTIVE_HIGH>; + hall-c-gpios = <&spin_header 49 GPIO_ACTIVE_HIGH>; + hall-sector-table = <5 1 0 3 4 2>; + hall-interpolation = "NONE"; + status = "okay"; + }; + + abz: incremental-encoder { + compatible = "shield-position-abz"; + position-sensor-name = "ABZ"; + motor = <&default_motor>; + direction-sign = <1>; + electrical-offset = <0x00000000>; + timer = <&timers3>; + counts-per-revolution = <1024>; + status = "okay"; + }; + + sincos: sin-cos-sensor { + compatible = "shield-position-sincos"; + position-sensor-name = "SINCOS"; + motor = <&default_motor>; + direction-sign = <1>; + electrical-offset = <0x00000000>; + sin-sensor-name = "ANALOG_SIN"; + cos-sensor-name = "ANALOG_COS"; + sin-spin-pin = <43>; + cos-spin-pin = <45>; + status = "okay"; + }; + }; +}; + +&timers3 { + status = "okay"; + + encoder { + pinctrl-0 = <&tim3_etr_pd2 &tim3_ch1_pc6 &tim3_ch2_pc7>; + pinctrl-names = "incremental_encoder"; + }; }; // UART / RS485 transceiver diff --git a/zephyr/boards/shields/ownverter/ownverter_v1_0_0.overlay b/zephyr/boards/shields/ownverter/ownverter_v1_0_0.overlay index c1a7f824..d89b16f9 100644 --- a/zephyr/boards/shields/ownverter/ownverter_v1_0_0.overlay +++ b/zephyr/boards/shields/ownverter/ownverter_v1_0_0.overlay @@ -15,6 +15,17 @@ shield-version = "v1.0.0"; }; + default_motor: default-motor { + /* + * Placeholder motor configuration. + * Applications can override these values from app.overlay without + * changing the shield description. + */ + compatible = "motor-config"; + pole-pairs = <1>; + status = "okay"; + }; + powershield: power-shield{ compatible = "power-leg"; default-frequency = <200000>; @@ -272,6 +283,46 @@ }; }; + shield-position-sensors { + hall: hall-sensor { + compatible = "shield-position-hall"; + position-sensor-name = "HALL"; + motor = <&default_motor>; + direction-sign = <1>; + electrical-offset = <0x00000000>; + hall-a-gpios = <&spin_header 7 GPIO_ACTIVE_HIGH>; + hall-b-gpios = <&spin_header 9 GPIO_ACTIVE_HIGH>; + hall-c-gpios = <&spin_header 49 GPIO_ACTIVE_HIGH>; + hall-sector-table = <5 1 0 3 4 2>; + hall-interpolation = "NONE"; + status = "okay"; + }; + + abz: incremental-encoder { + compatible = "shield-position-abz"; + position-sensor-name = "ABZ"; + motor = <&default_motor>; + direction-sign = <1>; + electrical-offset = <0x00000000>; + timer = <&timers3>; + counts-per-revolution = <1024>; + status = "okay"; + }; + + sincos: sin-cos-sensor { + compatible = "shield-position-sincos"; + position-sensor-name = "SINCOS"; + motor = <&default_motor>; + direction-sign = <1>; + electrical-offset = <0x00000000>; + sin-sensor-name = "ANALOG_SIN"; + cos-sensor-name = "ANALOG_COS"; + sin-spin-pin = <43>; + cos-spin-pin = <45>; + status = "okay"; + }; + }; + shield-safety-thresholds { /* Voltage thresholds */ @@ -363,6 +414,15 @@ }; }; +&timers3 { + status = "okay"; + + encoder { + pinctrl-0 = <&tim3_etr_pd2 &tim3_ch1_pc6 &tim3_ch2_pc7>; + pinctrl-names = "incremental_encoder"; + }; +}; + // UART / RS485 transceiver &usart3 { diff --git a/zephyr/boards/shields/ownverter/ownverter_v1_1_0.overlay b/zephyr/boards/shields/ownverter/ownverter_v1_1_0.overlay index e1fc80eb..95bd34ee 100644 --- a/zephyr/boards/shields/ownverter/ownverter_v1_1_0.overlay +++ b/zephyr/boards/shields/ownverter/ownverter_v1_1_0.overlay @@ -15,6 +15,17 @@ shield-version = "v1.1.0"; }; + default_motor: default-motor { + /* + * Placeholder motor configuration. + * Applications can override these values from app.overlay without + * changing the shield description. + */ + compatible = "motor-config"; + pole-pairs = <1>; + status = "okay"; + }; + powershield: power-shield{ compatible = "power-leg"; default-frequency = <200000>; @@ -272,6 +283,53 @@ }; }; + shield-position-sensors { + /* + * Position sensors are described here because the Ownverter shield + * owns the external connector wiring. For now, Sin/Cos channels also + * remain listed in `shield-sensors` so the current ADC-based flow + * keeps working unchanged. + */ + + hall: hall-sensor { + compatible = "shield-position-hall"; + position-sensor-name = "HALL"; + motor = <&default_motor>; + direction-sign = <1>; + electrical-offset = <0x00000000>; + hall-a-gpios = <&spin_header 7 GPIO_ACTIVE_HIGH>; + hall-b-gpios = <&spin_header 9 GPIO_ACTIVE_HIGH>; + hall-c-gpios = <&spin_header 49 GPIO_ACTIVE_HIGH>; + hall-sector-table = <5 1 0 3 4 2>; + hall-interpolation = "NONE"; + status = "okay"; + }; + + abz: incremental-encoder { + compatible = "shield-position-abz"; + position-sensor-name = "ABZ"; + motor = <&default_motor>; + direction-sign = <1>; + electrical-offset = <0x00000000>; + timer = <&timers3>; + counts-per-revolution = <1024>; + status = "okay"; + }; + + sincos: sin-cos-sensor { + compatible = "shield-position-sincos"; + position-sensor-name = "SINCOS"; + motor = <&default_motor>; + direction-sign = <1>; + electrical-offset = <0x00000000>; + sin-sensor-name = "ANALOG_SIN"; + cos-sensor-name = "ANALOG_COS"; + sin-spin-pin = <43>; + cos-spin-pin = <45>; + status = "okay"; + }; + }; + shield-safety-thresholds { /* Voltage thresholds */ @@ -363,6 +421,15 @@ }; }; +&timers3 { + status = "okay"; + + encoder { + pinctrl-0 = <&tim3_etr_pd2 &tim3_ch1_pc6 &tim3_ch2_pc7>; + pinctrl-names = "incremental_encoder"; + }; +}; + // UART / RS485 transceiver &usart3 { diff --git a/zephyr/dts/bindings/motor-config.yaml b/zephyr/dts/bindings/motor-config.yaml new file mode 100644 index 00000000..c4ad606c --- /dev/null +++ b/zephyr/dts/bindings/motor-config.yaml @@ -0,0 +1,9 @@ +description: Default motor configuration shared by position sensors + +compatible: "motor-config" + +properties: + pole-pairs: + type: int + required: false + description: Number of motor pole pairs. diff --git a/zephyr/dts/bindings/shield-position-abz.yaml b/zephyr/dts/bindings/shield-position-abz.yaml new file mode 100644 index 00000000..401c8d48 --- /dev/null +++ b/zephyr/dts/bindings/shield-position-abz.yaml @@ -0,0 +1,34 @@ +description: Incremental encoder wiring and default configuration exposed by a shield + +compatible: "shield-position-abz" + +properties: + position-sensor-name: + type: string + required: true + description: Name under which the position sensor is referred to in user code. + + motor: + type: phandle + required: false + description: Optional reference to the default motor configuration node. + + direction-sign: + type: int + required: false + description: Direction convention used by this position sensor, typically 1 or -1. + + electrical-offset: + type: int + required: false + description: Electrical offset for this position sensor, stored as an IEEE754 float32 bit pattern. + + timer: + type: phandle + required: true + description: Timer instance used in incremental encoder mode. + + counts-per-revolution: + type: int + required: false + description: Counts per revolution of this incremental encoder. diff --git a/zephyr/dts/bindings/shield-position-hall.yaml b/zephyr/dts/bindings/shield-position-hall.yaml new file mode 100644 index 00000000..953c1cfb --- /dev/null +++ b/zephyr/dts/bindings/shield-position-hall.yaml @@ -0,0 +1,49 @@ +description: Hall position sensor wiring and default configuration exposed by a shield + +compatible: "shield-position-hall" + +properties: + position-sensor-name: + type: string + required: true + description: Name under which the position sensor is referred to in user code. + + motor: + type: phandle + required: false + description: Optional reference to the default motor configuration node. + + direction-sign: + type: int + required: false + description: Direction convention used by this position sensor, typically 1 or -1. + + electrical-offset: + type: int + required: false + description: Electrical offset for this position sensor, stored as an IEEE754 float32 bit pattern. + + hall-a-gpios: + type: phandle-array + required: true + description: Hall channel A GPIO. + + hall-b-gpios: + type: phandle-array + required: true + description: Hall channel B GPIO. + + hall-c-gpios: + type: phandle-array + required: true + description: Hall channel C GPIO. + + hall-sector-table: + type: array + required: false + description: Hall-state to sector mapping table with 6 entries for raw states 001 to 110. Invalid states 000 and 111 are handled by the driver. + + hall-interpolation: + type: string + required: false + description: Default Hall interpolation mode. diff --git a/zephyr/dts/bindings/shield-position-sincos.yaml b/zephyr/dts/bindings/shield-position-sincos.yaml new file mode 100644 index 00000000..4b22b62f --- /dev/null +++ b/zephyr/dts/bindings/shield-position-sincos.yaml @@ -0,0 +1,44 @@ +description: Sin/Cos position sensor wiring and default configuration exposed by a shield + +compatible: "shield-position-sincos" + +properties: + position-sensor-name: + type: string + required: true + description: Name under which the position sensor is referred to in user code. + + motor: + type: phandle + required: false + description: Optional reference to the default motor configuration node. + + direction-sign: + type: int + required: false + description: Direction convention used by this position sensor, typically 1 or -1. + + electrical-offset: + type: int + required: false + description: Electrical offset for this position sensor, stored as an IEEE754 float32 bit pattern. + + sin-spin-pin: + type: int + required: true + description: Spin header pin used for the analog sine channel. + + cos-spin-pin: + type: int + required: true + description: Spin header pin used for the analog cosine channel. + + sin-sensor-name: + type: string + required: true + description: Matching shield sensor name used to acquire the analog sine channel. + + cos-sensor-name: + type: string + required: true + description: Matching shield sensor name used to acquire the analog cosine channel. diff --git a/zephyr/modules/owntech_shield_api/zephyr/CMakeLists.txt b/zephyr/modules/owntech_shield_api/zephyr/CMakeLists.txt index 75ddf41f..8ac1d2c3 100644 --- a/zephyr/modules/owntech_shield_api/zephyr/CMakeLists.txt +++ b/zephyr/modules/owntech_shield_api/zephyr/CMakeLists.txt @@ -1,6 +1,8 @@ if(CONFIG_OWNTECH_SHIELD_API) # Select directory to add to the include path zephyr_include_directories(./public_api) + zephyr_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../../../owntech/lib/USB/control_library/src) + zephyr_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../../../owntech/lib/STLink/control_library/src) # Define the current folder as a Zephyr library zephyr_library() @@ -9,6 +11,7 @@ if(CONFIG_OWNTECH_SHIELD_API) # Main API zephyr_library_sources( + ./src/Position.cpp ./src/Sensors.cpp ./src/Power.cpp ./src/power_init.cpp diff --git a/zephyr/modules/owntech_shield_api/zephyr/public_api/ShieldAPI.cpp b/zephyr/modules/owntech_shield_api/zephyr/public_api/ShieldAPI.cpp index fcc79242..bc7709b2 100644 --- a/zephyr/modules/owntech_shield_api/zephyr/public_api/ShieldAPI.cpp +++ b/zephyr/modules/owntech_shield_api/zephyr/public_api/ShieldAPI.cpp @@ -33,6 +33,7 @@ ShieldAPI shield; PowerAPI ShieldAPI::power; SensorsAPI ShieldAPI::sensors; +PositionAPI ShieldAPI::position; #ifdef CONFIG_OWNTECH_NGND_DRIVER NgndHAL ShieldAPI::ngnd; diff --git a/zephyr/modules/owntech_shield_api/zephyr/public_api/ShieldAPI.h b/zephyr/modules/owntech_shield_api/zephyr/public_api/ShieldAPI.h index 81dff6f0..39ca816c 100644 --- a/zephyr/modules/owntech_shield_api/zephyr/public_api/ShieldAPI.h +++ b/zephyr/modules/owntech_shield_api/zephyr/public_api/ShieldAPI.h @@ -30,6 +30,7 @@ #include "../src/Sensors.h" +#include "../src/Position.h" #include "../src/Power.h" @@ -44,6 +45,11 @@ class ShieldAPI */ static SensorsAPI sensors; + /** + * @brief Contains all the functions to interact with shield position sensors + */ + static PositionAPI position; + /** * @brief Contains all the functions to drive shield power capabilities */ diff --git a/zephyr/modules/owntech_shield_api/zephyr/src/Position.cpp b/zephyr/modules/owntech_shield_api/zephyr/src/Position.cpp new file mode 100644 index 00000000..21407e21 --- /dev/null +++ b/zephyr/modules/owntech_shield_api/zephyr/src/Position.cpp @@ -0,0 +1,987 @@ +/* + * Copyright (c) 2026-present LAAS-CNRS + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 2.1 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + * + * SPDX-License-Identifier: LGPL-2.1 + */ + +#include "Position.h" + +#include +#include + +#include + +#include "filters.h" +#include "trigo.h" + +#define POSITION_DEFAULT_DIRECTION_SIGN 1 +#define POSITION_DEFAULT_POLE_PAIRS 1 +#define POSITION_DEFAULT_COUNTS_PER_REVOLUTION 1024U +#define POSITION_DEFAULT_ELECTRICAL_OFFSET_RAW 0x00000000U +#define POSITION_DEFAULT_HALL_INTERPOLATION HALL_INTERPOLATION_NONE +#define POSITION_DEFAULT_HALL_SECTOR_TABLE {5U, 1U, 0U, 3U, 4U, 2U} + +#define POSITION_HALL_A(node_id) \ + DT_GPIO_PIN_BY_IDX(node_id, hall_a_gpios, 0) + +#define POSITION_HALL_B(node_id) \ + DT_GPIO_PIN_BY_IDX(node_id, hall_b_gpios, 0) + +#define POSITION_HALL_C(node_id) \ + DT_GPIO_PIN_BY_IDX(node_id, hall_c_gpios, 0) + +#define POSITION_TIMER_FROM_NODE(timer_node_id) \ + (DT_SAME_NODE(timer_node_id, DT_NODELABEL(timers4)) ? TIMER4 : TIMER3) + +#define POSITION_TIMER(node_id) \ + POSITION_TIMER_FROM_NODE(DT_PHANDLE(node_id, timer)) + +#define POSITION_TIMER_IS_SUPPORTED(node_id) \ + (DT_SAME_NODE(DT_PHANDLE(node_id, timer), DT_NODELABEL(timers3)) || \ + DT_SAME_NODE(DT_PHANDLE(node_id, timer), DT_NODELABEL(timers4))) + +#define POSITION_ASSERT_SUPPORTED_TIMER(node_id) \ + BUILD_ASSERT(POSITION_TIMER_IS_SUPPORTED(node_id), \ + "Position API only supports timers3 or timers4 for incremental encoder mode"); + +#define POSITION_SIN_PIN(node_id) \ + DT_PROP(node_id, sin_spin_pin) + +#define POSITION_COS_PIN(node_id) \ + DT_PROP(node_id, cos_spin_pin) + +#define POSITION_SIN_SENSOR(node_id) \ + DT_STRING_TOKEN(node_id, sin_sensor_name) + +#define POSITION_COS_SENSOR(node_id) \ + DT_STRING_TOKEN(node_id, cos_sensor_name) + +#define MOTOR_NODE(node_id) DT_PHANDLE(node_id, motor) + +#define POSITION_HAS_MOTOR(node_id) \ + DT_NODE_HAS_PROP(node_id, motor) + +#define MOTOR_HAS_PROP(node_id, prop_name) \ + COND_CODE_1(POSITION_HAS_MOTOR(node_id), \ + (DT_NODE_HAS_PROP(MOTOR_NODE(node_id), prop_name)), \ + (0)) + +#define SENSOR_COUNTS_PER_REVOLUTION(node_id) \ + DT_PROP_OR(node_id, counts_per_revolution, \ + POSITION_DEFAULT_COUNTS_PER_REVOLUTION) + +#define SENSOR_DIRECTION_SIGN(node_id) \ + DT_PROP_OR(node_id, direction_sign, \ + POSITION_DEFAULT_DIRECTION_SIGN) + +#define SENSOR_ELECTRICAL_OFFSET(node_id) \ + DT_PROP_OR(node_id, electrical_offset, \ + POSITION_DEFAULT_ELECTRICAL_OFFSET_RAW) + +#define MOTOR_POLE_PAIRS(node_id) \ + COND_CODE_1(POSITION_HAS_MOTOR(node_id), \ + (DT_PROP_OR(MOTOR_NODE(node_id), pole_pairs, \ + POSITION_DEFAULT_POLE_PAIRS)), \ + (POSITION_DEFAULT_POLE_PAIRS)) + +#define SENSOR_HALL_TABLE(node_id) \ + DT_PROP_OR(node_id, hall_sector_table, \ + POSITION_DEFAULT_HALL_SECTOR_TABLE) + +#define MOTOR_HALL_INTERPOLATION_TOKEN(interpolation_token) \ + DT_CAT(HALL_INTERPOLATION_, interpolation_token) + +#define SENSOR_HALL_INTERPOLATION(node_id) \ + (MOTOR_HALL_INTERPOLATION_TOKEN( \ + DT_STRING_TOKEN_OR(node_id, hall_interpolation, NONE))) + +#define POSITION_COMMON_PROP(node_id, sensor_type_value) \ + .name_string = DT_PROP(node_id, position_sensor_name), \ + .name = DT_STRING_TOKEN(node_id, position_sensor_name), \ + .type = sensor_type_value, \ + .has_motor = POSITION_HAS_MOTOR(node_id), \ + .has_direction_sign = DT_NODE_HAS_PROP(node_id, direction_sign), \ + .has_pole_pairs = MOTOR_HAS_PROP(node_id, pole_pairs), \ + .has_electrical_offset = DT_NODE_HAS_PROP(node_id, electrical_offset), \ + .has_counts_per_revolution = DT_NODE_HAS_PROP(node_id, counts_per_revolution),\ + .has_hall_sector_table = DT_NODE_HAS_PROP(node_id, hall_sector_table), \ + .has_hall_interpolation = DT_NODE_HAS_PROP(node_id, hall_interpolation), \ + .motor = \ + { \ + .direction_sign = SENSOR_DIRECTION_SIGN(node_id), \ + .pole_pairs = MOTOR_POLE_PAIRS(node_id), \ + .electrical_offset = int2float_t{.raw_value = SENSOR_ELECTRICAL_OFFSET(node_id)}.float_value \ + } + +#define POSITION_WRITE_PROP_HALL(node_id) \ + { \ + POSITION_COMMON_PROP(node_id, HALL_TYPE), \ + .configuration = \ + { \ + .hall = \ + { \ + .hall_a_pin = POSITION_HALL_A(node_id), \ + .hall_b_pin = POSITION_HALL_B(node_id), \ + .hall_c_pin = POSITION_HALL_C(node_id), \ + .hall_sector_table = SENSOR_HALL_TABLE(node_id), \ + .hall_interpolation = SENSOR_HALL_INTERPOLATION(node_id), \ + } \ + } \ + }, + +#define POSITION_WRITE_PROP_ABZ(node_id) \ + { \ + POSITION_COMMON_PROP(node_id, ABZ_TYPE), \ + .configuration = \ + { \ + .incremental_encoder = \ + { \ + .timer = POSITION_TIMER(node_id), \ + .counts_per_revolution = SENSOR_COUNTS_PER_REVOLUTION(node_id), \ + } \ + } \ + }, + +#define POSITION_WRITE_PROP_SINCOS(node_id) \ + { \ + POSITION_COMMON_PROP(node_id, SINCOS_TYPE), \ + .configuration = \ + { \ + .sincos = \ + { \ + .sin_pin = POSITION_SIN_PIN(node_id), \ + .cos_pin = POSITION_COS_PIN(node_id), \ + .sin_sensor_name = POSITION_SIN_SENSOR(node_id), \ + .cos_sensor_name = POSITION_COS_SENSOR(node_id), \ + } \ + } \ + }, + +#define POSITION_COUNT(node_id) +1 +#define DT_POSITION_SENSOR_COUNT \ + (0 \ + DT_FOREACH_STATUS_OKAY(shield_position_hall, POSITION_COUNT) \ + DT_FOREACH_STATUS_OKAY(shield_position_abz, POSITION_COUNT) \ + DT_FOREACH_STATUS_OKAY(shield_position_sincos, POSITION_COUNT)) + +DT_FOREACH_STATUS_OKAY(shield_position_abz, POSITION_ASSERT_SUPPORTED_TIMER) + +#if DT_POSITION_SENSOR_COUNT > 0 +PositionAPI::position_sensor_dt_data_t PositionAPI::dt_position_sensors_props[] = +{ + DT_FOREACH_STATUS_OKAY(shield_position_hall, POSITION_WRITE_PROP_HALL) + DT_FOREACH_STATUS_OKAY(shield_position_abz, POSITION_WRITE_PROP_ABZ) + DT_FOREACH_STATUS_OKAY(shield_position_sincos, POSITION_WRITE_PROP_SINCOS) +}; +#else +PositionAPI::position_sensor_dt_data_t PositionAPI::dt_position_sensors_props[1] = {}; +#endif + +bool PositionAPI::initialized = false; +PositionAPI::position_sensor_dt_data_t* PositionAPI::active_sensor = nullptr; + +typedef struct +{ + bool initialized; + float32_t elapsed_time; + float32_t hall_time_since_transition; + uint8_t hall_state; + uint8_t hall_state_previous; + int16_t hall_sector; + int16_t hall_sector_previous; + float32_t hall_transition_time; + PllAngle hall_pll; + PllDatas hall_pll_datas; + float32_t hall_pll_sampling_period; + uint32_t incremental_encoder_count; + uint32_t incremental_encoder_count_previous; + float32_t signed_sincos_angle; + float32_t signed_sincos_angle_previous; + float32_t mechanical_angle; + float32_t electrical_angle; + float32_t mechanical_speed; + float32_t electrical_speed; +} position_runtime_state_t; + +static position_runtime_state_t runtime_state = {}; + +static int8_t normalize_hall_delta(uint8_t current_sector, uint8_t previous_sector) +{ + int8_t delta = (int8_t)current_sector - (int8_t)previous_sector; + + if (delta > 3) + { + delta -= 6; + } + else if (delta < -3) + { + delta += 6; + } + + return delta; +} + +static bool is_valid_hall_sector_table(const uint8_t hall_sector_table[6]) +{ + bool seen[6] = {false}; + + for (uint8_t i = 0; i < 6U; i++) + { + uint8_t sector = hall_sector_table[i]; + if (sector > 5U) + { + return false; + } + + if (seen[sector] == true) + { + return false; + } + + seen[sector] = true; + } + + return true; +} + +static int32_t normalize_encoder_delta(uint32_t current_count, + uint32_t previous_count, + uint32_t counts_per_revolution) +{ + int32_t delta = (int32_t)current_count - (int32_t)previous_count; + + if (counts_per_revolution == 0U) + { + return delta; + } + + int32_t half_counts_per_revolution = (int32_t)(counts_per_revolution / 2U); + + if (delta > half_counts_per_revolution) + { + delta -= (int32_t)counts_per_revolution; + } + else if (delta < -half_counts_per_revolution) + { + delta += (int32_t)counts_per_revolution; + } + + return delta; +} + +static float32_t wrap_pm_pi(float32_t angle) +{ + while (angle > PI) + { + angle -= 2.0F * PI; + } + + while (angle < -PI) + { + angle += 2.0F * PI; + } + + return angle; +} + +bool PositionAPI::init(position_sensor_t sensor_name) +{ + position_sensor_dt_data_t* sensor_prop = findSensor(sensor_name); + if (sensor_prop == nullptr) + { + return false; + } + + if (configureSelectedSensorHardware(sensor_prop) == false) + { + return false; + } + + active_sensor = sensor_prop; + resetRuntimeState(); + warnIfUsingDefaultConfiguration(sensor_prop); + initialized = true; + + return true; +} + +bool PositionAPI::initDefault() +{ +#if DT_HAS_CHOSEN(owntech_position_sensor) + return init(DT_STRING_TOKEN(DT_CHOSEN(owntech_position_sensor), + position_sensor_name)); +#else + return false; +#endif +} + +bool PositionAPI::isInitialized() +{ + return initialized; +} + +bool PositionAPI::update(float32_t sampling_period) +{ + if ((active_sensor == nullptr) || (sampling_period <= 0.0F)) + { + return false; + } + + const position_motor_config_t* motor_config = getMotorConfig(); + float32_t electrical_offset = + (motor_config != nullptr) ? motor_config->electrical_offset : 0.0F; + uint8_t pole_pairs = (motor_config != nullptr) ? motor_config->pole_pairs : 0U; + int8_t direction_sign = (motor_config != nullptr) ? motor_config->direction_sign : 1; + runtime_state.elapsed_time += sampling_period; + + switch (active_sensor->type) + { + case HALL_TYPE: + { + const hall_position_sensor_config_t* hall_config = getHallConfig(); + if (hall_config == nullptr) + { + return false; + } + + runtime_state.hall_time_since_transition += sampling_period; + + uint8_t hall_state = getHallState(); + runtime_state.hall_state = hall_state; + + if ((hall_state == 0U) || (hall_state == 7U)) + { + return false; + } + + uint8_t hall_sector = hall_config->hall_sector_table[hall_state - 1U]; + runtime_state.hall_sector = hall_sector; + float32_t base_mechanical_angle = + ot_modulo_2pi((float32_t)direction_sign * + ((PI / 3.0F) * (float32_t)hall_sector)); + + float32_t base_electrical_angle = + ot_modulo_2pi(base_mechanical_angle + + electrical_offset); + + if (hall_config->hall_interpolation == HALL_INTERPOLATION_LINEAR) + { + if ((runtime_state.initialized == false) || + (runtime_state.hall_pll_sampling_period != sampling_period)) + { + runtime_state.hall_pll.init(sampling_period, 10.0F, 0.04F); + runtime_state.hall_pll.reset(base_electrical_angle); + runtime_state.hall_pll_sampling_period = sampling_period; + } + + runtime_state.hall_pll_datas = + runtime_state.hall_pll.calculateWithReturn(base_electrical_angle); + runtime_state.electrical_angle = runtime_state.hall_pll_datas.angle; + runtime_state.electrical_speed = runtime_state.hall_pll_datas.w; + } + else + { + runtime_state.electrical_angle = base_electrical_angle; + + if (runtime_state.initialized == false) + { + runtime_state.electrical_speed = 0.0F; + } + else if (hall_sector != (uint8_t)runtime_state.hall_sector_previous) + { + int8_t sector_delta = normalize_hall_delta( + hall_sector, + (uint8_t)runtime_state.hall_sector_previous); + + if (runtime_state.hall_time_since_transition > 0.0F) + { + runtime_state.electrical_speed = + (float32_t)direction_sign * + ((PI / 3.0F) * (float32_t)sector_delta) / + runtime_state.hall_time_since_transition; + } + + runtime_state.hall_time_since_transition = 0.0F; + } + } + + runtime_state.hall_state_previous = hall_state; + runtime_state.hall_sector_previous = hall_sector; + + if (pole_pairs > 0U) + { + if (hall_config->hall_interpolation == HALL_INTERPOLATION_LINEAR) + { + float32_t signed_mechanical_angle = + (runtime_state.electrical_angle - electrical_offset) / + (float32_t)pole_pairs; + runtime_state.mechanical_angle = + ot_modulo_2pi(signed_mechanical_angle); + } + else + { + runtime_state.mechanical_angle = + ot_modulo_2pi(base_mechanical_angle / + (float32_t)pole_pairs); + } + runtime_state.mechanical_speed = + runtime_state.electrical_speed / (float32_t)pole_pairs; + } + else + { + runtime_state.mechanical_angle = 0.0F; + runtime_state.mechanical_speed = 0.0F; + } + + break; + } + + case ABZ_TYPE: + { + const incremental_encoder_position_sensor_config_t* encoder_config = + getIncrementalEncoderConfig(); + if ((encoder_config == nullptr) || + (encoder_config->counts_per_revolution == 0U)) + { + return false; + } + + uint32_t current_count = + getIncrementalEncoderValue() % + encoder_config->counts_per_revolution; + + if (runtime_state.initialized == false) + { + runtime_state.incremental_encoder_count_previous = current_count; + } + + int32_t delta_count = normalize_encoder_delta( + current_count, + runtime_state.incremental_encoder_count_previous, + encoder_config->counts_per_revolution); + + runtime_state.incremental_encoder_count = current_count; + runtime_state.incremental_encoder_count_previous = current_count; + + float32_t signed_mechanical_angle = + (float32_t)direction_sign * + (2.0F * PI * (float32_t)current_count / + (float32_t)encoder_config->counts_per_revolution); + + runtime_state.mechanical_angle = ot_modulo_2pi(signed_mechanical_angle); + runtime_state.mechanical_speed = + (float32_t)direction_sign * + (2.0F * PI * (float32_t)delta_count) / + ((float32_t)encoder_config->counts_per_revolution * + sampling_period); + + runtime_state.electrical_angle = + ot_modulo_2pi((float32_t)pole_pairs * + runtime_state.mechanical_angle + + electrical_offset); + runtime_state.electrical_speed = + (float32_t)pole_pairs * runtime_state.mechanical_speed; + + break; + } + + case SINCOS_TYPE: + { + uint8_t sin_valid = 0U; + uint8_t cos_valid = 0U; + float32_t sin_value = getSinValue(&sin_valid); + float32_t cos_value = getCosValue(&cos_valid); + + if ((sin_valid == 0U) || (cos_valid == 0U) || + (sin_value == NO_VALUE) || (cos_value == NO_VALUE)) + { + return false; + } + + float32_t raw_mechanical_angle = atan2f(sin_value, cos_value); + if (raw_mechanical_angle < 0.0F) + { + raw_mechanical_angle += 2.0F * PI; + } + + float32_t signed_mechanical_angle = + (float32_t)direction_sign * raw_mechanical_angle; + + if (runtime_state.initialized == false) + { + runtime_state.signed_sincos_angle_previous = signed_mechanical_angle; + } + + float32_t delta_angle = + wrap_pm_pi(signed_mechanical_angle - + runtime_state.signed_sincos_angle_previous); + + runtime_state.signed_sincos_angle = signed_mechanical_angle; + runtime_state.signed_sincos_angle_previous = signed_mechanical_angle; + runtime_state.mechanical_angle = + ot_modulo_2pi(signed_mechanical_angle); + runtime_state.mechanical_speed = delta_angle / sampling_period; + runtime_state.electrical_angle = + ot_modulo_2pi((float32_t)pole_pairs * + runtime_state.mechanical_angle + + electrical_offset); + runtime_state.electrical_speed = + (float32_t)pole_pairs * runtime_state.mechanical_speed; + + break; + } + + case POSITION_SENSOR_TYPE_UNDEFINED: + default: + return false; + } + + runtime_state.initialized = true; + return true; +} + +position_sensor_t PositionAPI::getActiveSensor() +{ + if (active_sensor == nullptr) + { + return UNDEFINED_POSITION_SENSOR; + } + + return active_sensor->name; +} + +position_sensor_type_t PositionAPI::getActiveSensorType() +{ + if (active_sensor == nullptr) + { + return POSITION_SENSOR_TYPE_UNDEFINED; + } + + return active_sensor->type; +} + +const position_motor_config_t* PositionAPI::getMotorConfig() +{ + if (active_sensor == nullptr) + { + return nullptr; + } + + return &active_sensor->motor; +} + +const hall_position_sensor_config_t* PositionAPI::getHallConfig() +{ + if ((active_sensor == nullptr) || (active_sensor->type != HALL_TYPE)) + { + return nullptr; + } + + return &active_sensor->configuration.hall; +} + +const incremental_encoder_position_sensor_config_t* +PositionAPI::getIncrementalEncoderConfig() +{ + if ((active_sensor == nullptr) || (active_sensor->type != ABZ_TYPE)) + { + return nullptr; + } + + return &active_sensor->configuration.incremental_encoder; +} + +const sincos_position_sensor_config_t* PositionAPI::getSinCosConfig() +{ + if ((active_sensor == nullptr) || (active_sensor->type != SINCOS_TYPE)) + { + return nullptr; + } + + return &active_sensor->configuration.sincos; +} + +uint8_t PositionAPI::getHallState() +{ + const hall_position_sensor_config_t* hall_config = getHallConfig(); + if (hall_config == nullptr) + { + return 0; + } + + uint8_t hall_a = spin.gpio.readPin(hall_config->hall_a_pin); + uint8_t hall_b = spin.gpio.readPin(hall_config->hall_b_pin); + uint8_t hall_c = spin.gpio.readPin(hall_config->hall_c_pin); + + return (uint8_t)(hall_a + (2U * hall_b) + (4U * hall_c)); +} + +uint32_t PositionAPI::getIncrementalEncoderValue() +{ + const incremental_encoder_position_sensor_config_t* incremental_encoder_config = + getIncrementalEncoderConfig(); + if (incremental_encoder_config == nullptr) + { + return 0U; + } + + return spin.timer.getIncrementalEncoderValue(incremental_encoder_config->timer); +} + +float32_t PositionAPI::getSinValue(uint8_t* dataValid) +{ + const sincos_position_sensor_config_t* sincos_config = getSinCosConfig(); + if (sincos_config == nullptr) + { + if (dataValid != nullptr) *dataValid = 0U; + return NO_VALUE; + } + + SensorsAPI sensors_api; + return sensors_api.getLatestValue(sincos_config->sin_sensor_name, dataValid); +} + +float32_t PositionAPI::getCosValue(uint8_t* dataValid) +{ + const sincos_position_sensor_config_t* sincos_config = getSinCosConfig(); + if (sincos_config == nullptr) + { + if (dataValid != nullptr) *dataValid = 0U; + return NO_VALUE; + } + + SensorsAPI sensors_api; + return sensors_api.getLatestValue(sincos_config->cos_sensor_name, dataValid); +} + +void PositionAPI::setDirectionSign(int8_t direction_sign) +{ + if (active_sensor == nullptr) + { + return; + } + + if ((direction_sign != -1) && (direction_sign != 1)) + { + printk("WARNING: invalid direction-sign %d for position sensor %s; expected -1 or 1.\n", + direction_sign, + active_sensor->name_string); + return; + } + + active_sensor->motor.direction_sign = direction_sign; + resetRuntimeState(); +} + +void PositionAPI::setPolePairs(uint8_t pole_pairs) +{ + if (active_sensor == nullptr) + { + return; + } + + if (pole_pairs == 0U) + { + printk("WARNING: invalid pole-pairs 0 for position sensor %s; expected a strictly positive value.\n", + active_sensor->name_string); + return; + } + + active_sensor->motor.pole_pairs = pole_pairs; + resetRuntimeState(); +} + +void PositionAPI::setElectricalOffset(float32_t electrical_offset) +{ + if (active_sensor == nullptr) + { + return; + } + + active_sensor->motor.electrical_offset = electrical_offset; + resetRuntimeState(); +} + +bool PositionAPI::setCountsPerRevolution(uint32_t counts_per_revolution) +{ + incremental_encoder_position_sensor_config_t* encoder_config = + const_cast( + getIncrementalEncoderConfig()); + if ((encoder_config == nullptr) || (counts_per_revolution == 0U)) + { + return false; + } + + encoder_config->counts_per_revolution = counts_per_revolution; + resetRuntimeState(); + return true; +} + +bool PositionAPI::setHallSectorTable(const uint8_t hall_sector_table[6]) +{ + hall_position_sensor_config_t* hall_config = + const_cast(getHallConfig()); + if ((hall_config == nullptr) || (hall_sector_table == nullptr)) + { + return false; + } + + if (is_valid_hall_sector_table(hall_sector_table) == false) + { + return false; + } + + for (uint8_t i = 0; i < 6U; i++) + { + hall_config->hall_sector_table[i] = hall_sector_table[i]; + } + resetRuntimeState(); + return true; +} + +bool PositionAPI::setHallInterpolation(hall_interpolation_t hall_interpolation) +{ + hall_position_sensor_config_t* hall_config = + const_cast(getHallConfig()); + if (hall_config == nullptr) + { + return false; + } + + hall_config->hall_interpolation = hall_interpolation; + resetRuntimeState(); + return true; +} + +uint32_t PositionAPI::getCountsPerRevolution() +{ + const incremental_encoder_position_sensor_config_t* incremental_encoder_config = + getIncrementalEncoderConfig(); + if (incremental_encoder_config == nullptr) + { + return 0U; + } + + return incremental_encoder_config->counts_per_revolution; +} + +int8_t PositionAPI::getDirectionSign() +{ + const position_motor_config_t* motor_config = getMotorConfig(); + if (motor_config == nullptr) + { + return 1; + } + + return motor_config->direction_sign; +} + +uint8_t PositionAPI::getPolePairs() +{ + const position_motor_config_t* motor_config = getMotorConfig(); + if (motor_config == nullptr) + { + return 0U; + } + + return motor_config->pole_pairs; +} + +float32_t PositionAPI::getElectricalOffset() +{ + const position_motor_config_t* motor_config = getMotorConfig(); + if (motor_config == nullptr) + { + return 0.0F; + } + + return motor_config->electrical_offset; +} + +const uint8_t* PositionAPI::getHallSectorTable() +{ + const hall_position_sensor_config_t* hall_config = getHallConfig(); + if (hall_config == nullptr) + { + return nullptr; + } + + return hall_config->hall_sector_table; +} + +hall_interpolation_t PositionAPI::getHallInterpolation() +{ + const hall_position_sensor_config_t* hall_config = getHallConfig(); + if (hall_config == nullptr) + { + return HALL_INTERPOLATION_NONE; + } + + return hall_config->hall_interpolation; +} + +float32_t PositionAPI::getMechanicalAngle() +{ + return runtime_state.mechanical_angle; +} + +float32_t PositionAPI::getElectricalAngle() +{ + return runtime_state.electrical_angle; +} + +float32_t PositionAPI::getMechanicalSpeed() +{ + return runtime_state.mechanical_speed; +} + +float32_t PositionAPI::getElectricalSpeed() +{ + return runtime_state.electrical_speed; +} + +void PositionAPI::warnIfUsingDefaultConfiguration( + position_sensor_dt_data_t* sensor_prop) +{ + if (sensor_prop == nullptr) + { + return; + } + + if (sensor_prop->has_motor == false) + { + printk("WARNING: position sensor %s has no motor configuration node; using default pole-pairs value 1.\n", + sensor_prop->name_string); + } + + if (sensor_prop->has_direction_sign == false) + { + printk("WARNING: position sensor %s is missing direction-sign; using default value 1.\n", + sensor_prop->name_string); + } + + if ((sensor_prop->has_motor == true) && + (sensor_prop->has_pole_pairs == false)) + { + printk("WARNING: position sensor %s references a motor node without pole-pairs; using default value 1.\n", + sensor_prop->name_string); + } + + if (sensor_prop->has_electrical_offset == false) + { + printk("WARNING: position sensor %s is missing electrical-offset; using default value 0.0.\n", + sensor_prop->name_string); + } + + if ((sensor_prop->type == ABZ_TYPE) && + (sensor_prop->has_counts_per_revolution == false)) + { + printk("WARNING: position sensor %s is missing counts-per-revolution; using default value 1024.\n", + sensor_prop->name_string); + } + + if (sensor_prop->type == HALL_TYPE) + { + if (sensor_prop->has_hall_sector_table == false) + { + printk("WARNING: position sensor %s is missing hall-sector-table; using default sequence {5, 1, 0, 3, 4, 2}.\n", + sensor_prop->name_string); + } + + if (sensor_prop->has_hall_interpolation == false) + { + printk("WARNING: position sensor %s is missing hall-interpolation; using NONE.\n", + sensor_prop->name_string); + } + } +} + +float32_t PositionAPI::getMechanicalPulsation() +{ + return runtime_state.mechanical_speed; +} + +float32_t PositionAPI::getElectricalPulsation() +{ + return runtime_state.electrical_speed; +} + +PositionAPI::position_sensor_dt_data_t* PositionAPI::findSensor( + position_sensor_t sensor_name) +{ +#if DT_POSITION_SENSOR_COUNT > 0 + for (uint8_t sensor_index = 0; sensor_index < DT_POSITION_SENSOR_COUNT; sensor_index++) + { + if (dt_position_sensors_props[sensor_index].name == sensor_name) + { + return &dt_position_sensors_props[sensor_index]; + } + } +#endif + + return nullptr; +} + +bool PositionAPI::configureSelectedSensorHardware( + position_sensor_dt_data_t* sensor_prop) +{ + if (sensor_prop == nullptr) + { + return false; + } + + switch (sensor_prop->type) + { + case HALL_TYPE: + spin.gpio.configurePin(sensor_prop->configuration.hall.hall_a_pin, INPUT); + spin.gpio.configurePin(sensor_prop->configuration.hall.hall_b_pin, INPUT); + spin.gpio.configurePin(sensor_prop->configuration.hall.hall_c_pin, INPUT); + return true; + + case ABZ_TYPE: + spin.timer.startLogIncrementalEncoder( + sensor_prop->configuration.incremental_encoder.timer); + return true; + + case SINCOS_TYPE: + { + SensorsAPI sensors_api; + int8_t sin_enable_status = sensors_api.enablePositionSensor( + sensor_prop->configuration.sincos.sin_sensor_name); + int8_t cos_enable_status = sensors_api.enablePositionSensor( + sensor_prop->configuration.sincos.cos_sensor_name); + + if ((sin_enable_status < 0) || (cos_enable_status < 0)) + { + printk("WARNING: failed to enable Sin/Cos acquisition for position sensor %s.\n", + sensor_prop->name_string); + return false; + } + + return true; + } + + case POSITION_SENSOR_TYPE_UNDEFINED: + default: + return false; + } +} + +void PositionAPI::resetRuntimeState() +{ + runtime_state = {}; + runtime_state.hall_pll.reset(0.0F); +} diff --git a/zephyr/modules/owntech_shield_api/zephyr/src/Position.h b/zephyr/modules/owntech_shield_api/zephyr/src/Position.h new file mode 100644 index 00000000..898724c0 --- /dev/null +++ b/zephyr/modules/owntech_shield_api/zephyr/src/Position.h @@ -0,0 +1,170 @@ +/* + * Copyright (c) 2026-present LAAS-CNRS + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 2.1 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + * + * SPDX-License-Identifier: LGPL-2.1 + */ + +#ifndef POSITION_H_ +#define POSITION_H_ + +#include +#include + +#include +#include + +#include "Sensors.h" +#include "SpinAPI.h" + +#define POSITION_SENSOR_TOKEN(node_id) \ + DT_STRING_TOKEN(node_id, position_sensor_name), + +#define POSITION_SENSOR_FOREACH_OKAY(fn) \ + DT_FOREACH_STATUS_OKAY(shield_position_hall, fn) \ + DT_FOREACH_STATUS_OKAY(shield_position_abz, fn) \ + DT_FOREACH_STATUS_OKAY(shield_position_sincos, fn) + +typedef enum +{ + UNDEFINED_POSITION_SENSOR = 0, + POSITION_SENSOR_FOREACH_OKAY(POSITION_SENSOR_TOKEN) +} position_sensor_t; + +typedef enum +{ + POSITION_SENSOR_TYPE_UNDEFINED = 0, + HALL_TYPE, + ABZ_TYPE, + SINCOS_TYPE +} position_sensor_type_t; + +typedef enum +{ + HALL_INTERPOLATION_NONE = 0, + HALL_INTERPOLATION_LINEAR +} hall_interpolation_t; + +typedef struct +{ + int8_t direction_sign; + uint8_t pole_pairs; + float32_t electrical_offset; +} position_motor_config_t; + +typedef struct +{ + uint8_t hall_a_pin; + uint8_t hall_b_pin; + uint8_t hall_c_pin; + uint8_t hall_sector_table[6]; + hall_interpolation_t hall_interpolation; +} hall_position_sensor_config_t; + +typedef struct +{ + timernumber_t timer; + uint32_t counts_per_revolution; +} incremental_encoder_position_sensor_config_t; + +typedef struct +{ + uint8_t sin_pin; + uint8_t cos_pin; + sensor_t sin_sensor_name; + sensor_t cos_sensor_name; +} sincos_position_sensor_config_t; + +class PositionAPI +{ +private: + typedef union + { + uint32_t raw_value; + float32_t float_value; + } int2float_t; + + typedef struct + { + const char* name_string; + position_sensor_t name; + position_sensor_type_t type; + bool has_motor; + bool has_direction_sign; + bool has_pole_pairs; + bool has_electrical_offset; + bool has_counts_per_revolution; + bool has_hall_sector_table; + bool has_hall_interpolation; + position_motor_config_t motor; + union + { + hall_position_sensor_config_t hall; + incremental_encoder_position_sensor_config_t incremental_encoder; + sincos_position_sensor_config_t sincos; + } configuration; + } position_sensor_dt_data_t; + +public: + bool init(position_sensor_t sensor_name); + bool initDefault(); + bool isInitialized(); + bool update(float32_t sampling_period); + + position_sensor_t getActiveSensor(); + position_sensor_type_t getActiveSensorType(); + + const position_motor_config_t* getMotorConfig(); + const hall_position_sensor_config_t* getHallConfig(); + const incremental_encoder_position_sensor_config_t* getIncrementalEncoderConfig(); + const sincos_position_sensor_config_t* getSinCosConfig(); + + uint8_t getHallState(); + uint32_t getIncrementalEncoderValue(); + float32_t getSinValue(uint8_t* dataValid = nullptr); + float32_t getCosValue(uint8_t* dataValid = nullptr); + + void setDirectionSign(int8_t direction_sign); + void setPolePairs(uint8_t pole_pairs); + void setElectricalOffset(float32_t electrical_offset); + bool setCountsPerRevolution(uint32_t counts_per_revolution); + bool setHallSectorTable(const uint8_t hall_sector_table[6]); + bool setHallInterpolation(hall_interpolation_t hall_interpolation); + + uint32_t getCountsPerRevolution(); + int8_t getDirectionSign(); + uint8_t getPolePairs(); + float32_t getElectricalOffset(); + const uint8_t* getHallSectorTable(); + hall_interpolation_t getHallInterpolation(); + float32_t getMechanicalAngle(); + float32_t getElectricalAngle(); + float32_t getMechanicalSpeed(); + float32_t getElectricalSpeed(); + float32_t getMechanicalPulsation(); + float32_t getElectricalPulsation(); + +private: + static position_sensor_dt_data_t dt_position_sensors_props[]; + static bool initialized; + static position_sensor_dt_data_t* active_sensor; + + position_sensor_dt_data_t* findSensor(position_sensor_t sensor_name); + bool configureSelectedSensorHardware(position_sensor_dt_data_t* sensor_prop); + void warnIfUsingDefaultConfiguration(position_sensor_dt_data_t* sensor_prop); + void resetRuntimeState(); +}; + +#endif /* POSITION_H_ */ diff --git a/zephyr/modules/owntech_shield_api/zephyr/src/Sensors.cpp b/zephyr/modules/owntech_shield_api/zephyr/src/Sensors.cpp index e8b6ccc6..2b7193a3 100644 --- a/zephyr/modules/owntech_shield_api/zephyr/src/Sensors.cpp +++ b/zephyr/modules/owntech_shield_api/zephyr/src/Sensors.cpp @@ -180,6 +180,36 @@ int8_t SensorsAPI::enableSensor(sensor_t sensor_name, adc_t adc_num) return DataAPI::enableChannel(sensor_info.adc_num, sensor_info.channel_num); } +int8_t SensorsAPI::enablePositionSensor(sensor_t sensor_name) +{ + if (initialized == false) + { + buildSensorListFromDeviceTree(); + } + + if (sensor_name == UNDEFINED_SENSOR) return ERROR_CHANNEL_NOT_FOUND; + + for (uint8_t adc_index = 0; adc_index < ADC_COUNT; adc_index++) + { + for (uint8_t sensor = 0; + sensor < available_sensors_count[adc_index]; + sensor++) + { + sensor_dt_data_t* current_sensor = + available_sensors_props[adc_index][sensor]; + + if (current_sensor->name == sensor_name) + { + enabled_sensors[((int)sensor_name) - 1] = current_sensor; + return DataAPI::enableChannel((adc_t)current_sensor->adc_number, + current_sensor->channel_number); + } + } + } + + return ERROR_CHANNEL_NOT_FOUND; +} + uint16_t* SensorsAPI::getRawValues(sensor_t sensor_name, uint32_t& number_of_values_acquired) { @@ -334,15 +364,15 @@ void SensorsAPI::enableDefaultOwnverterSensors() /* Creates the list of measurements of the ADC 1 */ this->enableSensor(V1_LOW, ADC_1); this->enableSensor(V2_LOW, ADC_1); - this->enableSensor(I3_LOW, ADC_1); this->enableSensor(V_HIGH, ADC_1); + this->enableSensor(I_HIGH, ADC_1); this->enableSensor(V_NEUTR, ADC_1); /* Creates the list of measurements of the ADC 2 */ this->enableSensor(I1_LOW, ADC_2); this->enableSensor(I2_LOW, ADC_2); + this->enableSensor(I3_LOW, ADC_2); this->enableSensor(V3_LOW, ADC_2); - this->enableSensor(I_HIGH, ADC_2); this->enableSensor(TEMP_SENSOR, ADC_2); /* Configure the pins of the temperature MUX */ diff --git a/zephyr/modules/owntech_shield_api/zephyr/src/Sensors.h b/zephyr/modules/owntech_shield_api/zephyr/src/Sensors.h index b09b42c0..a5088545 100644 --- a/zephyr/modules/owntech_shield_api/zephyr/src/Sensors.h +++ b/zephyr/modules/owntech_shield_api/zephyr/src/Sensors.h @@ -142,6 +142,21 @@ class SensorsAPI */ int8_t enableSensor(sensor_t sensor_name, adc_t adc_number); + /** + * @brief Enable a position-related shield sensor using the first ADC + * declared in device tree for this semantic sensor name. + * + * @note This helper is intended for the Position API, where semantic + * signals such as Sin/Cos are expected to map to a single ADC path. + * If several ADC mappings exist, the first ADC visited by the + * internal ADC-order iteration is selected. + * + * @param[in] sensor_name Name of the sensor using enumeration sensor_t. + * + * @return 0 if the sensor was correctly enabled, negative value otherwise. + */ + int8_t enablePositionSensor(sensor_t sensor_name); + /** * @brief Function to access the acquired data for specified sensor. *