diff --git a/src/current_sense/hardware_specific/zephyr/zephyr_mcu.cpp b/src/current_sense/hardware_specific/zephyr/zephyr_mcu.cpp new file mode 100644 index 00000000..eb0c3d24 --- /dev/null +++ b/src/current_sense/hardware_specific/zephyr/zephyr_mcu.cpp @@ -0,0 +1,79 @@ + +#include "../../hardware_api.h" + +#if defined(ARDUINO_ARCH_ZEPHYR) + +#include +#include "../../../communication/SimpleFOCDebug.h" + +// Inline current sensing on the Arduino Zephyr core (ArduinoCore-zephyr) uses the standard +// Arduino analogRead()/analogReadResolution() API, which reads the ADC channels declared in +// the `io-channels` property of the board's `zephyr,user` devicetree node. +// +// Low-side current sensing needs the ADC conversions to be triggered in sync with the PWM +// timer. That synchronization is not available through the Arduino wiring layer, so only +// inline current sensing is supported here. + +// ADC read resolution used for current sensing. Overridable from the build environment. +#ifndef SIMPLEFOC_ZEPHYR_ADC_RESOLUTION +#define SIMPLEFOC_ZEPHYR_ADC_RESOLUTION 12 +#endif + +// ADC reference voltage in volts. Overridable from the build environment to match the +// board's analog reference. +#ifndef SIMPLEFOC_ZEPHYR_ADC_VOLTAGE +#define SIMPLEFOC_ZEPHYR_ADC_VOLTAGE 3.3f +#endif + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA, const void* cs_params){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; +} + +// function configuring the ADC for inline current sensing +void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ + _UNUSED(driver_params); + + analogReadResolution(SIMPLEFOC_ZEPHYR_ADC_RESOLUTION); + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + const float adc_range = (float)(1UL << SIMPLEFOC_ZEPHYR_ADC_RESOLUTION); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (SIMPLEFOC_ZEPHYR_ADC_VOLTAGE) / adc_range + }; + + return params; +} + +// low-side current sensing is not supported through the Arduino Zephyr wiring API +float _readADCVoltageLowSide(const int pinA, const void* cs_params){ + _UNUSED(pinA); + _UNUSED(cs_params); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported on Zephyr core!"); + return 0.0f; +} + +void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ + _UNUSED(driver_params); + _UNUSED(pinA); + _UNUSED(pinB); + _UNUSED(pinC); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported on Zephyr core!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; +} + +void* _driverSyncLowSide(void* driver_params, void* cs_params){ + _UNUSED(driver_params); + return cs_params; +} + +void _startADC3PinConversionLowSide(){ } + +#endif diff --git a/src/drivers/hardware_specific/zephyr/README.md b/src/drivers/hardware_specific/zephyr/README.md new file mode 100644 index 00000000..e789f665 --- /dev/null +++ b/src/drivers/hardware_specific/zephyr/README.md @@ -0,0 +1,70 @@ +# Arduino Zephyr core support + +Hardware-specific implementation for the [Arduino Zephyr core](https://github.com/arduino/ArduinoCore-zephyr) +(`ArduinoCore-zephyr`). It is selected automatically when the `ARDUINO_ARCH_ZEPHYR` +macro is defined by the build. + +- Driver PWM: [`zephyr_mcu.cpp`](zephyr_mcu.cpp) +- Inline current sense: [`../../../current_sense/hardware_specific/zephyr/zephyr_mcu.cpp`](../../../current_sense/hardware_specific/zephyr/zephyr_mcu.cpp) + +## What is supported + +| Feature | Status | Notes | +|---|---|---| +| 1-PWM / 2-PWM / 3-PWM / 4-PWM | ✅ | Zephyr PWM timers, runtime frequency control | +| 6-PWM (complementary) | ❌ | needs hardware dead-time; not portable via `pwm_set_dt()` | +| Inline current sensing | ✅ | via `analogRead()` | +| Low-side current sensing | ❌ | needs ADC/PWM synchronization; not exposed by the wiring API | + +## How it works + +The PWM is generated by driving the **Zephyr PWM timers directly** through the native +`pwm_set_pulse_dt()` API — the same approach used by the +[Arduino_HardwareServo](https://github.com/arduino-libraries/Arduino_HardwareServo) library. +This gives full **runtime control of the PWM frequency**: the requested `pwm_frequency` +(Hz) is converted to a period in nanoseconds and stored once, at configuration time, into a +per-pin copy of the channel's `pwm_dt_spec`. Each duty-cycle update then only pushes the new +pulse width with `pwm_set_pulse_dt()`, reusing that stored period. + +The PWM channels are taken from the `pwms` property of the board's `zephyr,user` devicetree +node and mapped to Arduino pin numbers through the `pwm-pin-gpios` property, exactly as the +Arduino Zephyr core does internally. + +If a requested pin is **not** mapped to a PWM channel in the devicetree, the driver falls +back to the wiring `analogWrite()` (which degrades to a digital HIGH/LOW output) and prints +a warning — so make sure your driver pins are declared under `pwms` / `pwm-pin-gpios`. + +Current sensing uses the standard `analogRead()` / `analogReadResolution()` API, reading the +ADC channels declared in `io-channels`. + +## Board devicetree overlay (important) + +Your **board overlay** must declare which pins can output PWM and which can be read by the +ADC. The PWM frequency itself is set at runtime by SimpleFOC via `pwm_set_dt()`, but each +`pwms` entry still needs a (nonzero) default `period`. + +Example `zephyr,user` overlay fragment: + +```dts +/ { + zephyr,user { + pwms = <&pwm0 0 PWM_HZ(25000) PWM_POLARITY_NORMAL>, + <&pwm0 1 PWM_HZ(25000) PWM_POLARITY_NORMAL>, + <&pwm0 2 PWM_HZ(25000) PWM_POLARITY_NORMAL>; + pwm-pin-gpios = <&gpio0 2 0>, <&gpio0 3 0>, <&gpio0 4 0>; + + io-channels = <&adc 0>, <&adc 1>, <&adc 2>; + adc-pin-gpios = <&gpio0 5 0>, <&gpio0 6 0>, <&gpio0 7 0>; + }; +}; +``` + +## Build-time options + +Override from your build environment / `build_flags` if needed: + +| Macro | Default | Meaning | +|---|---|---| +| `SIMPLEFOC_ZEPHYR_PWM_RESOLUTION` | `12` | `analogWrite` resolution (bits), fallback pins only | +| `SIMPLEFOC_ZEPHYR_ADC_RESOLUTION` | `12` | `analogRead` resolution (bits) | +| `SIMPLEFOC_ZEPHYR_ADC_VOLTAGE` | `3.3f` | ADC reference voltage (V) for current sensing | diff --git a/src/drivers/hardware_specific/zephyr/zephyr_mcu.cpp b/src/drivers/hardware_specific/zephyr/zephyr_mcu.cpp new file mode 100644 index 00000000..52ae93d4 --- /dev/null +++ b/src/drivers/hardware_specific/zephyr/zephyr_mcu.cpp @@ -0,0 +1,226 @@ + +#include "../../hardware_api.h" + +#if defined(ARDUINO_ARCH_ZEPHYR) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino Zephyr core (ArduinoCore-zephyr)") +#pragma message("") + +// The Arduino Zephyr core (https://github.com/arduino/ArduinoCore-zephyr) runs on top of +// the Zephyr RTOS. To get true runtime control of the PWM frequency (which the wiring +// analogWrite() API does not offer, as its period is fixed by the devicetree), we drive the +// Zephyr PWM timers directly through the native pwm_set_pulse_dt() API - the same method used +// by the Arduino_HardwareServo library (https://github.com/arduino-libraries/Arduino_HardwareServo). +// +// The requested frequency is converted to a period (ns) and stored, once at configuration +// time, into a per-pin copy of the channel's pwm_dt_spec. The duty-cycle updates then only +// push the new pulse width with pwm_set_pulse_dt(), reusing that stored period. +// +// The PWM channels are taken from the `pwms` property of the board's `zephyr,user` devicetree +// node, and mapped to Arduino pin numbers through the `pwm-pin-gpios` property. Make sure the +// driver pins you use are declared there in your board overlay (see README.md). + +#include +#include +#include +#include // zephyr::arduino::init_dev_apply_channel_pinctrl(), state_pin_index_from_spec_index() + +// Build a pwm_dt_spec table and a pin->channel map from the devicetree, exactly like +// Arduino_HardwareServo / the Arduino Zephyr core do internally. +#if DT_NODE_HAS_PROP(DT_PATH(zephyr_user), pwms) && DT_NODE_HAS_PROP(DT_PATH(zephyr_user), pwm_pin_gpios) + +#define SIMPLEFOC_ZEPHYR_HAS_PWM 1 + +#define _SIMPLEFOC_PWM_DT_SPEC(n, p, i) PWM_DT_SPEC_GET_BY_IDX(n, i), +#define _SIMPLEFOC_PWM_PINS(n, p, i) \ + DIGITAL_PIN_GPIOS_FIND_PIN(DT_REG_ADDR(DT_PHANDLE_BY_IDX(DT_PATH(zephyr_user), p, i)), \ + DT_PHA_BY_IDX(DT_PATH(zephyr_user), p, i, pin)), + +static const struct pwm_dt_spec _foc_pwm[] = { + DT_FOREACH_PROP_ELEM(DT_PATH(zephyr_user), pwms, _SIMPLEFOC_PWM_DT_SPEC) +}; + +// maps Arduino digital pin numbers to indices into _foc_pwm[] +static const pin_size_t _foc_pwm_pins[] = { + DT_FOREACH_PROP_ELEM(DT_PATH(zephyr_user), pwm_pin_gpios, _SIMPLEFOC_PWM_PINS) +}; + +// returns the index of the pin into the pwm spec table, or -1 if the pin is not a PWM channel +static int _foc_pwm_index(pin_size_t pinNumber) { + for (size_t i = 0; i < ARRAY_SIZE(_foc_pwm_pins); i++) { + if (_foc_pwm_pins[i] == pinNumber) return (int)i; + } + return -1; +} + +#endif // devicetree has pwms + + +// analogWrite resolution used only for the fallback (pins not mapped to a PWM channel in +// the devicetree). Overridable from the build environment. +#ifndef SIMPLEFOC_ZEPHYR_PWM_RESOLUTION +#define SIMPLEFOC_ZEPHYR_PWM_RESOLUTION 12 +#endif + +// hardware specific parameters for the Zephyr driver +typedef struct ZephyrDriverParams { + int pins[6]; // Arduino pin numbers + bool has_pwm[6]; // true if pins[i] maps to a hardware PWM channel +#ifdef SIMPLEFOC_ZEPHYR_HAS_PWM + struct pwm_dt_spec pwm[6];// per-pin channel spec copy, with .period set to the requested one +#endif + long pwm_frequency; // requested PWM frequency [Hz] + uint32_t period_ns; // PWM period in nanoseconds = 1e9 / pwm_frequency + float dead_zone; + uint32_t pwm_range; // max value for the analogWrite fallback = 2^resolution - 1 +} ZephyrDriverParams; + + +// default PWM frequency if none (or an invalid one) is requested +#define SIMPLEFOC_ZEPHYR_DEFAULT_PWM_FREQ 25000L + +static inline float _clampf(float v, float lo, float hi) { + return v < lo ? lo : (v > hi ? hi : v); +} + +// write a [0,1] duty cycle either through the Zephyr PWM timer (pulse only, reusing the +// period stored in the spec) or, for unmapped pins, through the analogWrite() fallback +static inline void _writeDuty(ZephyrDriverParams* p, int i, float dc) { + dc = _clampf(dc, 0.0f, 1.0f); +#ifdef SIMPLEFOC_ZEPHYR_HAS_PWM + if (p->has_pwm[i]) { + uint32_t pulse_ns = (uint32_t)(dc * (float)p->pwm[i].period); + pwm_set_pulse_dt(&p->pwm[i], pulse_ns); + return; + } +#endif + analogWrite(p->pins[i], (int)(dc * (float)p->pwm_range)); +} + +// common configuration helper for all N-PWM modes +static ZephyrDriverParams* _configurePWM(long pwm_frequency, const int* pins, int n) { + if (pwm_frequency <= 0) pwm_frequency = SIMPLEFOC_ZEPHYR_DEFAULT_PWM_FREQ; + + analogWriteResolution(SIMPLEFOC_ZEPHYR_PWM_RESOLUTION); + + ZephyrDriverParams* params = new ZephyrDriverParams(); + params->pwm_frequency = pwm_frequency; + params->period_ns = (uint32_t)(1000000000UL / (uint32_t)pwm_frequency); + params->dead_zone = NOT_SET; + params->pwm_range = (1UL << SIMPLEFOC_ZEPHYR_PWM_RESOLUTION) - 1UL; + + for (int i = 0; i < 6; i++) { params->pins[i] = NOT_SET; params->has_pwm[i] = false; } + + for (int i = 0; i < n; i++) { + params->pins[i] = pins[i]; +#ifdef SIMPLEFOC_ZEPHYR_HAS_PWM + int idx = _foc_pwm_index((pin_size_t)pins[i]); + if (idx >= 0) { + // Route the timer channel to the physical pin. The Arduino Zephyr core leaves the pins + // as plain GPIO at boot and applies the PWM pin mux lazily inside analogWrite(); we must + // do the same or the timer runs but its output never reaches the pin. + zephyr::arduino::init_dev_apply_channel_pinctrl( + _foc_pwm[idx].dev, + zephyr::arduino::state_pin_index_from_spec_index(_foc_pwm, idx)); + if (!pwm_is_ready_dt(&_foc_pwm[idx])) + SIMPLEFOC_DEBUG("ZEPHYR: PWM device not ready for pin: ", pins[i]); + + params->pwm[i] = _foc_pwm[idx]; // copy the devicetree channel spec ... + params->pwm[i].period = params->period_ns; // ... and apply the requested frequency + params->has_pwm[i] = true; + continue; + } + SIMPLEFOC_DEBUG("ZEPHYR: pin not mapped to a PWM channel, using analogWrite fallback: ", pins[i]); +#else + SIMPLEFOC_DEBUG("ZEPHYR: no `pwms` in the devicetree, using analogWrite fallback (no frequency control)."); +#endif + pinMode(pins[i], OUTPUT); // fallback (non-PWM) pins only + } + return params; +} + + +// Configuring PWM - 1PWM setting (single phase) +void* _configure1PWM(long pwm_frequency, const int pinA) { + const int pins[] = { pinA }; + return _configurePWM(pwm_frequency, pins, 1); +} + +// Configuring PWM - 2PWM setting (Stepper) +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + const int pins[] = { pinA, pinB }; + return _configurePWM(pwm_frequency, pins, 2); +} + +// Configuring PWM - 3PWM setting (BLDC) +void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { + const int pins[] = { pinA, pinB, pinC }; + return _configurePWM(pwm_frequency, pins, 3); +} + +// Configuring PWM - 4PWM setting (Stepper) +void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + const int pins[] = { pin1A, pin1B, pin2A, pin2B }; + return _configurePWM(pwm_frequency, pins, 4); +} + +// Configuring PWM - 6PWM setting (BLDC, complementary) +// +// Complementary 6-PWM with hardware dead-time is timer/board specific and cannot be produced +// portably: pwm_set_pulse_dt() drives each channel independently and inserts no dead-time, so +// a software complementary pair would risk shoot-through. 6-PWM is therefore reported as +// unsupported on this core - use two 3-PWM half-bridges configured in the devicetree instead. +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + _UNUSED(pwm_frequency); + _UNUSED(dead_zone); + _UNUSED(pinA_h); + _UNUSED(pinA_l); + _UNUSED(pinB_h); + _UNUSED(pinB_l); + _UNUSED(pinC_h); + _UNUSED(pinC_l); + + SIMPLEFOC_DEBUG("ZEPHYR: 6-PWM not supported. Use a 3-PWM driver, or hardware complementary PWM via the devicetree."); + return SIMPLEFOC_DRIVER_INIT_FAILED; +} + + +// Writing PWM duty cycles ------------------------------------------------------ + +void _writeDutyCycle1PWM(float dc_a, void* params) { + ZephyrDriverParams* p = (ZephyrDriverParams*)params; + _writeDuty(p, 0, dc_a); +} + +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { + ZephyrDriverParams* p = (ZephyrDriverParams*)params; + _writeDuty(p, 0, dc_a); + _writeDuty(p, 1, dc_b); +} + +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params) { + ZephyrDriverParams* p = (ZephyrDriverParams*)params; + _writeDuty(p, 0, dc_a); + _writeDuty(p, 1, dc_b); + _writeDuty(p, 2, dc_c); +} + +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params) { + ZephyrDriverParams* p = (ZephyrDriverParams*)params; + _writeDuty(p, 0, dc_1a); + _writeDuty(p, 1, dc_1b); + _writeDuty(p, 2, dc_2a); + _writeDuty(p, 3, dc_2b); +} + +// 6-PWM is not supported on this core (see _configure6PWM above) +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params) { + _UNUSED(dc_a); + _UNUSED(dc_b); + _UNUSED(dc_c); + _UNUSED(phase_state); + _UNUSED(params); +} + +#endif