From 600555eb5db7a6f8002df98214b1fa8ec242db34 Mon Sep 17 00:00:00 2001 From: "Miguel I." Date: Mon, 8 Sep 2025 21:05:07 +0200 Subject: [PATCH 1/6] first draft --- .vscode/settings.json | 4 +- boards/Mi_labs/phf000_board/phf000_board.dts | 5 + .../phf000_board/phf000_board_defconfig | 5 +- drivers/actuator/actuator.c | 190 +++++++++++++++++- drivers/actuator/actuator.h | 33 ++- prj.conf | 4 +- src/main.c | 8 +- 7 files changed, 222 insertions(+), 27 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 968ec96..790639d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -3,6 +3,8 @@ "${workspaceFolder}/build_4/PHF000-Firmware": "Launch PHF000-Firmware" }, "files.associations": { - "nrfx_temp.h": "c" + "nrfx_temp.h": "c", + "device.h": "c", + "counter.h": "c" } } \ No newline at end of file diff --git a/boards/Mi_labs/phf000_board/phf000_board.dts b/boards/Mi_labs/phf000_board/phf000_board.dts index 9aa324b..5181f87 100644 --- a/boards/Mi_labs/phf000_board/phf000_board.dts +++ b/boards/Mi_labs/phf000_board/phf000_board.dts @@ -183,4 +183,9 @@ zephyr_udc0: &usbd { &temp { compatible = "nordic,nrf-temp"; status = "okay"; +}; + +/* Enable Timer3 as counter backend */ +&timer3 { + status = "okay"; }; \ No newline at end of file diff --git a/boards/Mi_labs/phf000_board/phf000_board_defconfig b/boards/Mi_labs/phf000_board/phf000_board_defconfig index 257eda8..8a2633c 100644 --- a/boards/Mi_labs/phf000_board/phf000_board_defconfig +++ b/boards/Mi_labs/phf000_board/phf000_board_defconfig @@ -35,4 +35,7 @@ CONFIG_CLOCK_CONTROL_NRF_K32SRC_XTAL=y CONFIG_SENSOR=y CONFIG_MULTITHREADING=y -CONFIG_NRFX_TEMP=y \ No newline at end of file +CONFIG_NRFX_TEMP=y + +CONFIG_COUNTER=y +CONFIG_NRFX_TIMER3=y diff --git a/drivers/actuator/actuator.c b/drivers/actuator/actuator.c index 5bcab22..cd0114c 100644 --- a/drivers/actuator/actuator.c +++ b/drivers/actuator/actuator.c @@ -1,21 +1,191 @@ #include "actuator.h" +#include #include -#include +#include +#include +#include +#include +LOG_MODULE_REGISTER(actuator, LOG_LEVEL_INF); + +/* === GPIOs from devicetree === */ #define USER_NODE DT_PATH(zephyr_user) -static const struct gpio_dt_spec do1 = GPIO_DT_SPEC_GET(USER_NODE, do1_gpios); -static const struct gpio_dt_spec do2 = GPIO_DT_SPEC_GET(USER_NODE, do2_gpios); +static const struct gpio_dt_spec do1 = GPIO_DT_SPEC_GET(USER_NODE, do1_gpios); +static const struct gpio_dt_spec do2 = GPIO_DT_SPEC_GET(USER_NODE, do2_gpios); static const struct gpio_dt_spec do_en = GPIO_DT_SPEC_GET(USER_NODE, do_en_gpios); -void digital_out_init(void) { +/* === Counter device (Timer3) === */ +#define COUNTER_NODE DT_NODELABEL(timer3) +static const struct device *counter_dev = DEVICE_DT_GET(COUNTER_NODE); + +/* === Constants === */ +#define MOTOR_FREQ_HZ 200 +#define MOTOR_PERIOD_US (USEC_PER_SEC / MOTOR_FREQ_HZ) + +#define SERVO_FREQ_HZ 50 +#define SERVO_PERIOD_US (USEC_PER_SEC / SERVO_FREQ_HZ) + +/* Servo duty cycle limits */ +#define SERVO_MIN_PULSE_US (SERVO_PERIOD_US * 5 / 100) /* 5% → 1 ms */ +#define SERVO_MAX_PULSE_US (SERVO_PERIOD_US * 10 / 100) /* 10% → 2 ms */ + +/* === State === */ +static enum actuator_mode current_mode; +static bool initialized = false; +static int motor_speed = 0; /* -100..100 */ +static int servo_angle = 0; /* -90..90 */ + +static struct counter_alarm_cfg alarm_cfg; +static bool active_phase = false; + +/* === Forward declarations === */ +static void pwm_isr(const struct device *dev, + uint8_t chan_id, + uint32_t ticks, + void *user_data); + +/* === Helpers === */ +static int configure_gpio(void) +{ + if (!device_is_ready(do1.port) || !device_is_ready(do2.port) || !device_is_ready(do_en.port)) { + LOG_ERR("GPIO device not ready"); + return -ENODEV; + } + gpio_pin_configure_dt(&do1, GPIO_OUTPUT_INACTIVE); gpio_pin_configure_dt(&do2, GPIO_OUTPUT_INACTIVE); gpio_pin_configure_dt(&do_en, GPIO_OUTPUT_INACTIVE); + return 0; } -void digital_out_set_do1(int state) { gpio_pin_set_dt(&do1, state); } -void digital_out_set_do2(int state) { gpio_pin_set_dt(&do2, state); } -void digital_out_set_do_en(int state) { gpio_pin_set_dt(&do_en, state); } -void digital_out_toggle_do1(void) { gpio_pin_toggle_dt(&do1); } -void digital_out_toggle_do2(void) { gpio_pin_toggle_dt(&do2); } -void digital_out_toggle_do_en(void) { gpio_pin_toggle_dt(&do_en); } +static void start_counter(uint32_t us) +{ + uint32_t now_ticks; + int err = counter_get_value(counter_dev, &now_ticks); + if (err) + { + LOG_ERR("Failed to get counter value (%d)", err); + return; + } + + uint32_t delay_ticks = counter_us_to_ticks(counter_dev, us); + uint32_t next_ticks = now_ticks + delay_ticks; + + alarm_cfg.flags = COUNTER_ALARM_CFG_ABSOLUTE; + alarm_cfg.ticks = next_ticks; + alarm_cfg.callback = pwm_isr; + alarm_cfg.user_data = NULL; + + counter_set_channel_alarm(counter_dev, 0, &alarm_cfg); +} + +/* === ISR callback === */ +static void pwm_isr(const struct device *dev, + uint8_t chan_id, + uint32_t ticks, + void *user_data) +{ + ARG_UNUSED(dev); + ARG_UNUSED(chan_id); + ARG_UNUSED(ticks); + ARG_UNUSED(user_data); + + if (current_mode == ACTUATOR_MODE_MOTOR) { + if (motor_speed == 0) { + /* Brake mode */ + gpio_pin_set_dt(&do1, 1); + gpio_pin_set_dt(&do2, 1); + return; + } + + if (!active_phase) { + /* Start ON phase */ + if (motor_speed > 0) { + gpio_pin_set_dt(&do1, 1); + gpio_pin_set_dt(&do2, 0); + } else { + gpio_pin_set_dt(&do1, 0); + gpio_pin_set_dt(&do2, 1); + } + uint32_t duty_us = (MOTOR_PERIOD_US * abs(motor_speed)) / 100; + start_counter(duty_us); + active_phase = true; + } else { + /* Start OFF (coast) phase */ + gpio_pin_set_dt(&do1, 0); + gpio_pin_set_dt(&do2, 0); + uint32_t off_time = MOTOR_PERIOD_US - (MOTOR_PERIOD_US * abs(motor_speed)) / 100; + start_counter(off_time); + active_phase = false; + } + } + else if (current_mode == ACTUATOR_MODE_SERVO) { + if (!active_phase) { + /* Start ON phase */ + gpio_pin_set_dt(&do1, 1); + gpio_pin_set_dt(&do2, 1); + uint32_t duty_us = SERVO_MIN_PULSE_US + + (servo_angle + 90) * (SERVO_MAX_PULSE_US - SERVO_MIN_PULSE_US) / 180; + start_counter(duty_us); + active_phase = true; + } else { + /* Start OFF phase */ + gpio_pin_set_dt(&do2, 0); + uint32_t off_time = SERVO_PERIOD_US - + (SERVO_MIN_PULSE_US + (servo_angle + 90) * (SERVO_MAX_PULSE_US - SERVO_MIN_PULSE_US) / 180); + start_counter(off_time); + active_phase = false; + } + } +} + +/* === API implementation === */ +int actuator_init(enum actuator_mode mode) +{ + int ret = configure_gpio(); + if (ret) return ret; + + if (!device_is_ready(counter_dev)) { + LOG_ERR("Counter device not ready"); + return -ENODEV; + } + + counter_start(counter_dev); + + current_mode = mode; + initialized = true; + active_phase = false; + LOG_INF("Actuator initialized in mode %d", mode); + + return 0; +} + +int actuator_enable(bool enable) +{ + if (!initialized) return -EACCES; + return gpio_pin_set_dt(&do_en, enable ? 1 : 0); +} + +int actuator_motor_set_speed(int speed_percent) +{ + if (!initialized || current_mode != ACTUATOR_MODE_MOTOR) return -EACCES; + if (speed_percent < -100) speed_percent = -100; + if (speed_percent > 100) speed_percent = 100; + + motor_speed = speed_percent; + active_phase = false; + start_counter(1); /* trigger ISR soon */ + return 0; +} + +int actuator_servo_set_angle(int angle_deg) +{ + if (!initialized || current_mode != ACTUATOR_MODE_SERVO) return -EACCES; + if (angle_deg < -90) angle_deg = -90; + if (angle_deg > 90) angle_deg = 90; + + servo_angle = angle_deg; + active_phase = false; + start_counter(1); /* trigger ISR soon */ + return 0; +} diff --git a/drivers/actuator/actuator.h b/drivers/actuator/actuator.h index c6e8357..440fc3f 100644 --- a/drivers/actuator/actuator.h +++ b/drivers/actuator/actuator.h @@ -1,12 +1,25 @@ -#ifndef ACTUATOR_H -#define ACTUATOR_H +#ifndef ACTUATOR_H_ +#define ACTUATOR_H_ -void digital_out_init(void); -void digital_out_set_do1(int state); -void digital_out_set_do2(int state); -void digital_out_set_do_en(int state); -void digital_out_toggle_do1(void); -void digital_out_toggle_do2(void); -void digital_out_toggle_do_en(void); +#include +#include +#include +#include -#endif // ACTUATOR_H +/* Actuator modes */ +enum actuator_mode { + ACTUATOR_MODE_MOTOR = 0, + ACTUATOR_MODE_SERVO, +}; + +/* Public API */ +int actuator_init(enum actuator_mode mode); +int actuator_enable(bool enable); + +/* Motor mode */ +int actuator_motor_set_speed(int speed_percent); // -100..100 % + +/* Servo mode */ +int actuator_servo_set_angle(int angle_deg); // -90..90 degrees + +#endif /* ACTUATOR_H_ */ diff --git a/prj.conf b/prj.conf index faa0667..9e83858 100644 --- a/prj.conf +++ b/prj.conf @@ -21,4 +21,6 @@ CONFIG_USB_CDC_ACM=y CONFIG_USB_DEVICE_PRODUCT="Zephyr USB console sample" # STEP 1.1 - Enable MCUboot -CONFIG_BOOTLOADER_MCUBOOT=y \ No newline at end of file +CONFIG_BOOTLOADER_MCUBOOT=y + +CONFIG_COUNTER=y \ No newline at end of file diff --git a/src/main.c b/src/main.c index dde1032..84e7261 100644 --- a/src/main.c +++ b/src/main.c @@ -27,10 +27,13 @@ int main(void) return 0; } + actuator_init(ACTUATOR_MODE_MOTOR); + actuator_enable(1); + + actuator_motor_set_speed(50); // -100..100 % /* Init modules */ led_init(); button_init(); - digital_out_init(); ret = battery_adc_init(); if (ret < 0) { @@ -95,9 +98,6 @@ int main(void) if (old_val != val) { out_en = !out_en; - digital_out_toggle_do1(); - digital_out_toggle_do2(); - digital_out_toggle_do_en(); led_toggle(1); // toggle LED1 on press old_val = val; } From 33c5f4654cbd6c0986a48b3166fd539f51126aa1 Mon Sep 17 00:00:00 2001 From: "Miguel I." Date: Mon, 8 Sep 2025 21:41:03 +0200 Subject: [PATCH 2/6] fixed again compilation, reduced to one the partitions of the flash. --- pm_static.yml | 31 +++++-------------------------- sysbuild.conf | 3 ++- 2 files changed, 7 insertions(+), 27 deletions(-) diff --git a/pm_static.yml b/pm_static.yml index cf6dd2e..b3a065d 100644 --- a/pm_static.yml +++ b/pm_static.yml @@ -23,40 +23,19 @@ mcuboot_primary: end_address: 0x36000 region: flash_primary size: 0x26000 - orig_span: + orig_span: &id001 - mcuboot_pad - app + span: *id001 + mcuboot_primary_app: - orig_span: + orig_span: &id002 - app region: flash_primary address: 0x10200 end_address: 0x36000 - region: flash_primary size: 0x25e00 - -mcuboot_secondary: - address: 0x36000 - end_address: 0x7c000 - span: [mcuboot_secondary_pad, mcuboot_secondary_app] - region: flash_primary - size: 0x26000 - -mcuboot_secondary_app: - address: 0x36200 - end_address: 0x7c000 - placement: - after: [mcuboot_secondary_pad] - region: flash_primary - size: 0x25e00 - -mcuboot_secondary_pad: - address: 0x36000 - end_address: 0x36200 - placement: - after: [mcuboot_primary] - region: flash_primary - size: 0x200 + span: *id002 storage: address: 0x7c000 diff --git a/sysbuild.conf b/sysbuild.conf index 721a76f..a0dceb0 100644 --- a/sysbuild.conf +++ b/sysbuild.conf @@ -1 +1,2 @@ -SB_CONFIG_BOOTLOADER_MCUBOOT=y \ No newline at end of file +SB_CONFIG_BOOTLOADER_MCUBOOT=y +SB_CONFIG_MCUBOOT_MODE_SINGLE_APP=y \ No newline at end of file From ac9e564d6ac6f434e90c4673f8e758ce54612a15 Mon Sep 17 00:00:00 2001 From: "Miguel I." Date: Mon, 8 Sep 2025 22:07:54 +0200 Subject: [PATCH 3/6] aparently working, more test is needed --- drivers/actuator/actuator.c | 22 +++++++++++++--------- src/main.c | 5 +++-- 2 files changed, 16 insertions(+), 11 deletions(-) diff --git a/drivers/actuator/actuator.c b/drivers/actuator/actuator.c index cd0114c..6d5a929 100644 --- a/drivers/actuator/actuator.c +++ b/drivers/actuator/actuator.c @@ -5,8 +5,7 @@ #include #include #include - -LOG_MODULE_REGISTER(actuator, LOG_LEVEL_INF); +#include /* === GPIOs from devicetree === */ #define USER_NODE DT_PATH(zephyr_user) @@ -48,7 +47,7 @@ static void pwm_isr(const struct device *dev, static int configure_gpio(void) { if (!device_is_ready(do1.port) || !device_is_ready(do2.port) || !device_is_ready(do_en.port)) { - LOG_ERR("GPIO device not ready"); + printk("GPIO device not ready"); return -ENODEV; } @@ -64,7 +63,7 @@ static void start_counter(uint32_t us) int err = counter_get_value(counter_dev, &now_ticks); if (err) { - LOG_ERR("Failed to get counter value (%d)", err); + printk("Failed to get counter value (%d)", err); return; } @@ -76,7 +75,12 @@ static void start_counter(uint32_t us) alarm_cfg.callback = pwm_isr; alarm_cfg.user_data = NULL; - counter_set_channel_alarm(counter_dev, 0, &alarm_cfg); + err = counter_set_channel_alarm(counter_dev, 0, &alarm_cfg); + if (err) + { + printk("Failed to set channel alarm (%d)", err); + return; + } } /* === ISR callback === */ @@ -146,7 +150,7 @@ int actuator_init(enum actuator_mode mode) if (ret) return ret; if (!device_is_ready(counter_dev)) { - LOG_ERR("Counter device not ready"); + printk("Counter device not ready"); return -ENODEV; } @@ -155,7 +159,7 @@ int actuator_init(enum actuator_mode mode) current_mode = mode; initialized = true; active_phase = false; - LOG_INF("Actuator initialized in mode %d", mode); + printk("Actuator initialized in mode %d", mode); return 0; } @@ -174,7 +178,7 @@ int actuator_motor_set_speed(int speed_percent) motor_speed = speed_percent; active_phase = false; - start_counter(1); /* trigger ISR soon */ + start_counter(100); /* trigger ISR soon */ return 0; } @@ -186,6 +190,6 @@ int actuator_servo_set_angle(int angle_deg) servo_angle = angle_deg; active_phase = false; - start_counter(1); /* trigger ISR soon */ + start_counter(100); /* trigger ISR soon */ return 0; } diff --git a/src/main.c b/src/main.c index 84e7261..44917ab 100644 --- a/src/main.c +++ b/src/main.c @@ -27,10 +27,11 @@ int main(void) return 0; } - actuator_init(ACTUATOR_MODE_MOTOR); + actuator_init(ACTUATOR_MODE_SERVO); actuator_enable(1); - actuator_motor_set_speed(50); // -100..100 % + actuator_servo_set_angle(100); + //actuator_motor_set_speed(-50); // -100..100 % /* Init modules */ led_init(); button_init(); From ca95fe9f36293617c416a1825370c74fd7ed13f3 Mon Sep 17 00:00:00 2001 From: "Miguel I." Date: Tue, 9 Sep 2025 20:01:15 +0200 Subject: [PATCH 4/6] Fixed adc battery value --- drivers/battery_adc/battery_adc.c | 2 +- src/main.c | 18 ++++++++++++++---- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/drivers/battery_adc/battery_adc.c b/drivers/battery_adc/battery_adc.c index cfbd0c0..1638a9b 100644 --- a/drivers/battery_adc/battery_adc.c +++ b/drivers/battery_adc/battery_adc.c @@ -43,7 +43,7 @@ int battery_measure_mv(int32_t *mv_out) { if (ret) return ret; ret = adc_raw_to_millivolts_dt(&adc_channel, &sample_buffer); - *mv_out = (int)((float)sample_buffer*((270.0f+110.0f)/270.0f)); + *mv_out = (int)((float)sample_buffer*((270.0f+110.0f)/110.0f)); gpio_pin_set_dt(&btt_meas_en, 0); // disable measurement diff --git a/src/main.c b/src/main.c index 44917ab..2da994b 100644 --- a/src/main.c +++ b/src/main.c @@ -27,10 +27,9 @@ int main(void) return 0; } - actuator_init(ACTUATOR_MODE_SERVO); - actuator_enable(1); + //actuator_init(ACTUATOR_MODE_MOTOR); - actuator_servo_set_angle(100); + //actuator_servo_set_angle(100); //actuator_motor_set_speed(-50); // -100..100 % /* Init modules */ led_init(); @@ -101,6 +100,17 @@ int main(void) out_en = !out_en; led_toggle(1); // toggle LED1 on press old_val = val; + + actuator_enable(1); + if(out_en){ + actuator_motor_set_speed(35); // -100..100 % + } + else{ + actuator_motor_set_speed(-35); // -100..100 % + } + k_sleep(K_MSEC(300)); + actuator_motor_set_speed(0); // -100..100 % + actuator_enable(0); } } else @@ -112,6 +122,6 @@ int main(void) printk("Button released\n"); } - k_sleep(K_MSEC(50)); + k_sleep(K_MSEC(100)); } } From f6298795671458c36132b79d19b3b161612d88e9 Mon Sep 17 00:00:00 2001 From: "Miguel I." Date: Tue, 9 Sep 2025 20:04:28 +0200 Subject: [PATCH 5/6] expanded main partition --- pm_static.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/pm_static.yml b/pm_static.yml index b3a065d..5408948 100644 --- a/pm_static.yml +++ b/pm_static.yml @@ -1,8 +1,8 @@ app: address: 0x10000 - end_address: 0x36000 + end_address: 0x7c000 region: flash_primary - size: 0x26000 + size: 0x6c000 mcuboot: address: 0x0 @@ -20,9 +20,9 @@ mcuboot_pad: mcuboot_primary: address: 0x10000 - end_address: 0x36000 + end_address: 0x7c000 region: flash_primary - size: 0x26000 + size: 0x6c000 orig_span: &id001 - mcuboot_pad - app @@ -33,8 +33,8 @@ mcuboot_primary_app: - app region: flash_primary address: 0x10200 - end_address: 0x36000 - size: 0x25e00 + end_address: 0x7c000 + size: 0x6be00 span: *id002 storage: From 108f71cde17a7d45e3040ea0582e1db33e630918 Mon Sep 17 00:00:00 2001 From: "Miguel I." Date: Wed, 10 Sep 2025 19:22:09 +0200 Subject: [PATCH 6/6] enabled motor test --- src/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.c b/src/main.c index 2da994b..d4f7cce 100644 --- a/src/main.c +++ b/src/main.c @@ -27,7 +27,7 @@ int main(void) return 0; } - //actuator_init(ACTUATOR_MODE_MOTOR); + actuator_init(ACTUATOR_MODE_MOTOR); //actuator_servo_set_angle(100); //actuator_motor_set_speed(-50); // -100..100 %