diff --git a/CMakeLists.txt b/CMakeLists.txt index 19f0522..ecfdbdc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE modules/filter/moving_average/moving_average.c modules/filter/notch/notch.c modules/filter/rate_limiter/rate_limiter.c + modules/filter/dead_zone/dead_zone.c modules/device/motor/mf4010v2.c modules/device/led/led.c modules/device/can/can_device.c @@ -96,6 +97,7 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/moving_average ${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/notch ${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/rate_limiter + ${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/dead_zone ${CMAKE_CURRENT_SOURCE_DIR}/modules/control/pid ${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/soft_i2c ${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/i2c diff --git a/CloudPlant.ioc b/CloudPlant.ioc index 10be2da..82c801a 100644 --- a/CloudPlant.ioc +++ b/CloudPlant.ioc @@ -19,7 +19,7 @@ CAN2.NART=ENABLE CAN2.Prescaler=6 FREERTOS.FootprintOK=true FREERTOS.IPParameters=Tasks01,configENABLE_FPU,configUSE_NEWLIB_REENTRANT,configMINIMAL_STACK_SIZE,configTOTAL_HEAP_SIZE,FootprintOK -FREERTOS.Tasks01=monitorTask,8,256,StartMonitorTask,Default,NULL,Dynamic,NULL,NULL;controlTask,24,256,StartControlTask,Default,NULL,Dynamic,NULL,NULL;sensorTask,40,256,StartSensorTask,Default,NULL,Dynamic,NULL,NULL +FREERTOS.Tasks01=monitorTask,8,256,StartMonitorTask,Default,NULL,Dynamic,NULL,NULL;controlTask,40,256,StartControlTask,Default,NULL,Dynamic,NULL,NULL;sensorTask,24,256,StartSensorTask,Default,NULL,Dynamic,NULL,NULL FREERTOS.configENABLE_FPU=1 FREERTOS.configMINIMAL_STACK_SIZE=256 FREERTOS.configTOTAL_HEAP_SIZE=32768 diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c index db4f58e..f449935 100644 --- a/Core/Src/freertos.c +++ b/Core/Src/freertos.c @@ -59,14 +59,14 @@ osThreadId_t controlTaskHandle; const osThreadAttr_t controlTask_attributes = { .name = "controlTask", .stack_size = 256 * 4, - .priority = (osPriority_t) osPriorityNormal, + .priority = (osPriority_t) osPriorityHigh, }; /* Definitions for sensorTask */ osThreadId_t sensorTaskHandle; const osThreadAttr_t sensorTask_attributes = { .name = "sensorTask", .stack_size = 256 * 4, - .priority = (osPriority_t) osPriorityHigh, + .priority = (osPriority_t) osPriorityNormal, }; /* Private function prototypes -----------------------------------------------*/ diff --git a/Core/Src/main.c b/Core/Src/main.c index 9895c8e..90572c5 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -21,10 +21,8 @@ #include "cmsis_os.h" #include "can.h" #include "i2c.h" -#include "stm32f4xx_hal_uart.h" #include "usart.h" #include "gpio.h" -#include /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ diff --git a/app/callback_task.c b/app/callback_task.c index 9ace8fc..3625414 100644 --- a/app/callback_task.c +++ b/app/callback_task.c @@ -1,8 +1,6 @@ #include "app.h" -#include "cmd_parser.h" #include "cmsis_os2.h" #include -#include #include "FreeRTOS.h" #include "queue.h" #include "stm32f4xx_hal.h" @@ -84,179 +82,110 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } -/* ============================================================================ - * Command handlers (registered with cmd_parser) - * ============================================================================ */ - -static int cmd_help(void *ctx, int argc, char **argv) -{ - (void)ctx; (void)argc; (void)argv; - log_printf("[cmd] available commands:\r\n" - " help ==> show this message\r\n" - " motor enable [yaw|pitch] ==> enable motor(s), default both\r\n" - " motor disable [yaw|pitch] ==> disable motor(s), default both\r\n" - " sensor enable ==> resume IMU data publishing\r\n" - " sensor disable ==> pause IMU data publishing\r\n" - " sensorcali acc enable ==> start accelerometer calibration\r\n" - " sensorcali acc disable ==> stop accelerometer calibration\r\n" - " sensorcali mag enable ==> start magnetometer calibration\r\n" - " sensorcali mag disable ==> stop magnetometer calibration\r\n" - " pid ==> show all PID coefficients\r\n" - " pid

==> set PID param (v÷10=实际值, motor disabled)\r\n" - " axis: roll|pitch, p: kp|ki|kd, v: float\r\n"); - return 0; -} - -static int cmd_motor(void *ctx, int argc, char **argv) -{ - (void)ctx; - if (argc < 2) return -1; - - if (strcmp(argv[1], "enable") == 0) { - if (argc == 2) { - gimbal_motor_enable(); - log_printf("[cmd] both motors enabled\r\n"); - } else if (strcmp(argv[2], "yaw") == 0) { - gimbal_motor_enable_id(GIMBAL_MOTOR_YAW); - log_printf("[cmd] yaw motor enabled\r\n"); - } else if (strcmp(argv[2], "pitch") == 0) { - gimbal_motor_enable_id(GIMBAL_MOTOR_PITCH); - log_printf("[cmd] pitch motor enabled\r\n"); - } else { - log_printf("[cmd] unknown motor: %s\r\n", argv[2]); - } - return 0; - } - - if (strcmp(argv[1], "disable") == 0) { - if (argc == 2) { - gimbal_motor_disable(); - log_printf("[cmd] both motors disabled\r\n"); - } else if (strcmp(argv[2], "yaw") == 0) { - gimbal_motor_disable_id(GIMBAL_MOTOR_YAW); - log_printf("[cmd] yaw motor disabled\r\n"); - } else if (strcmp(argv[2], "pitch") == 0) { - gimbal_motor_disable_id(GIMBAL_MOTOR_PITCH); - log_printf("[cmd] pitch motor disabled\r\n"); - } else { - log_printf("[cmd] unknown motor: %s\r\n", argv[2]); - } - return 0; - } - - return -1; -} - -static int cmd_sensor(void *ctx, int argc, char **argv) -{ - (void)ctx; - if (argc < 2) return -1; - - if (strcmp(argv[1], "enable") == 0) { - sensor_enable(); - log_printf("[cmd] sensor enabled\r\n"); - } else if (strcmp(argv[1], "disable") == 0) { - sensor_disable(); - log_printf("[cmd] sensor disabled\r\n"); - } else { - return -1; - } - return 0; -} - -static int cmd_sensorcali(void *ctx, int argc, char **argv) -{ - (void)ctx; - if (argc < 3) return -1; - - if (strcmp(argv[1], "acc") == 0) { - if (strcmp(argv[2], "enable") == 0) { - sensor_acc_cali_enable(); - log_printf("[cmd] acc calibration started\r\n"); - } else if (strcmp(argv[2], "disable") == 0) { - sensor_acc_cali_disable(); - log_printf("[cmd] acc calibration stopped\r\n"); - } else { - return -1; - } - } else if (strcmp(argv[1], "mag") == 0) { - if (strcmp(argv[2], "enable") == 0) { - sensor_mag_cali_enable(); - log_printf("[cmd] mag calibration started\r\n"); - } else if (strcmp(argv[2], "disable") == 0) { - sensor_mag_cali_disable(); - log_printf("[cmd] mag calibration stopped\r\n"); - } else { - return -1; - } - } else { - return -1; - } - return 0; -} - -static int cmd_pid(void *ctx, int argc, char **argv) -{ - (void)ctx; - if (argc == 1) { - float kp, ki, kd; - gimbal_get_pid("roll", &kp, &ki, &kd); - log_printf("[cmd] Roll: Kp=%d Ki=%d Kd=%d\r\n", - (int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f)); - gimbal_get_pid("pitch", &kp, &ki, &kd); - log_printf("[cmd] Pitch: Kp=%d Ki=%d Kd=%d\r\n", - (int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f)); - return 0; - } - - if (argc == 4) { - if (gimbal_is_motor_enabled()) { - log_printf("[cmd] ERROR: disable motor first (motor disable)\r\n"); - return 0; - } - char *end; - long int_val = strtol(argv[3], &end, 10); - if (*end != '\0' || end == argv[3]) { - log_printf("[cmd] usage: pid \r\n"); - return 0; - } - float value = (float)int_val / 10.0f; - if (gimbal_set_pid(argv[1], argv[2], value) == 0) { - log_printf("[cmd] pid.%s.%s = %ld\r\n", argv[1], argv[2], int_val); - } else { - log_printf("[cmd] usage: pid \r\n"); - } - return 0; - } - - return -1; -} - void callback_task(void) { char cmd_buf[MAX_CMD_LEN]; - char disp_buf[MAX_CMD_LEN]; - cmd_parser_t parser; g_cmd_parser.state = USER_CMD_IDLE; g_cmd_parser.index = 0; g_cmd_parser.cmd_queue = g_cmd_queue; - cmd_parser_init(&parser); - cmd_parser_register(&parser, "help", cmd_help); - cmd_parser_register(&parser, "motor", cmd_motor); - cmd_parser_register(&parser, "sensor", cmd_sensor); - cmd_parser_register(&parser, "sensorcali", cmd_sensorcali); - cmd_parser_register(&parser, "pid", cmd_pid); - while (1) { osStatus_t qstat = osMessageQueueGet(g_cmd_queue, cmd_buf, NULL, osWaitForever); - if (qstat != osOK) continue; - if (cmd_buf[0] == '\0') continue; - - memcpy(disp_buf, cmd_buf, MAX_CMD_LEN); - if (cmd_parser_dispatch(&parser, cmd_buf, NULL) != 0) { - log_printf("[cmd] unknown: %s\r\n", disp_buf); + if (qstat != osOK) { + continue; + } + if (strcmp(cmd_buf, "help") == 0) { + log_printf("[cmd] available commands:\r\n" + " help ==> show this message\r\n" + " motor enable [yaw|pitch] ==> enable motor(s), default both\r\n" + " motor disable [yaw|pitch] ==> disable motor(s), default both\r\n" + " sensor enable ==> resume IMU data publishing\r\n" + " sensor disable ==> pause IMU data publishing\r\n" + " sensorcali acc enable ==> start accelerometer calibration\r\n" + " sensorcali acc disable ==> stop accelerometer calibration\r\n" + " sensorcali mag enable ==> start magnetometer calibration\r\n" + " sensorcali mag disable ==> stop magnetometer calibration\r\n" + " pid ==> show all PID coefficients\r\n" + " pid

==> set PID param (v÷10=实际值, motor disabled)\r\n" + " axis: roll|pitch, p: kp|ki|kd, v: float\r\n"); + } else if (strncmp(cmd_buf, "motor enable", 12) == 0) { + const char *p = cmd_buf + 12; + while (*p == ' ') p++; + if (*p == '\0') { + gimbal_motor_enable(); + log_printf("[cmd] both motors enabled\r\n"); + } else if (strcmp(p, "yaw") == 0) { + gimbal_motor_enable_id(GIMBAL_MOTOR_YAW); + log_printf("[cmd] yaw motor enabled\r\n"); + } else if (strcmp(p, "pitch") == 0) { + gimbal_motor_enable_id(GIMBAL_MOTOR_PITCH); + log_printf("[cmd] pitch motor enabled\r\n"); + } else { + log_printf("[cmd] unknown motor: %s\r\n", p); + } + } else if (strncmp(cmd_buf, "motor disable", 13) == 0) { + const char *p = cmd_buf + 13; + while (*p == ' ') p++; + if (*p == '\0') { + gimbal_motor_disable(); + log_printf("[cmd] both motors disabled\r\n"); + } else if (strcmp(p, "yaw") == 0) { + gimbal_motor_disable_id(GIMBAL_MOTOR_YAW); + log_printf("[cmd] yaw motor disabled\r\n"); + } else if (strcmp(p, "pitch") == 0) { + gimbal_motor_disable_id(GIMBAL_MOTOR_PITCH); + log_printf("[cmd] pitch motor disabled\r\n"); + } else { + log_printf("[cmd] unknown motor: %s\r\n", p); + } + } else if (strcmp(cmd_buf, "sensor enable") == 0) { + sensor_enable(); + log_printf("[cmd] sensor enabled\r\n"); + } else if (strcmp(cmd_buf, "sensor disable") == 0) { + sensor_disable(); + log_printf("[cmd] sensor disabled\r\n"); + } else if (strcmp(cmd_buf, "sensorcali acc enable") == 0) { + sensor_acc_cali_enable(); + log_printf("[cmd] acc calibration started\r\n"); + } else if (strcmp(cmd_buf, "sensorcali acc disable") == 0) { + sensor_acc_cali_disable(); + log_printf("[cmd] acc calibration stopped\r\n"); + } else if (strcmp(cmd_buf, "sensorcali mag enable") == 0) { + sensor_mag_cali_enable(); + log_printf("[cmd] mag calibration started\r\n"); + } else if (strcmp(cmd_buf, "sensorcali mag disable") == 0) { + sensor_mag_cali_disable(); + log_printf("[cmd] mag calibration stopped\r\n"); + } else if (strcmp(cmd_buf, "pid") == 0) { + /* 查看当前 PID 系数（整数显示，值 = 实际系数 × 10） */ + float kp, ki, kd; + gimbal_get_pid("roll", &kp, &ki, &kd); + log_printf("[cmd] Roll: Kp=%d Ki=%d Kd=%d\r\n", + (int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f)); + gimbal_get_pid("pitch", &kp, &ki, &kd); + log_printf("[cmd] Pitch: Kp=%d Ki=%d Kd=%d\r\n", + (int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f)); + } else if (strncmp(cmd_buf, "pid ", 4) == 0) { + /* 调参：pid （value ÷ 10 = 实际系数） */ + char axis[8], param[8]; + int int_val; + if (sscanf(cmd_buf, "pid %7s %7s %d", axis, param, &int_val) == 3) { + if (gimbal_is_motor_enabled()) { + log_printf("[cmd] ERROR: disable motor first (motor disable)\r\n"); + } else { + float value = (float)int_val / 10.0f; + if (gimbal_set_pid(axis, param, value) == 0) { + log_printf("[cmd] pid.%s.%s = %d\r\n", axis, param, int_val); + } else { + log_printf("[cmd] usage: pid \r\n"); + } + } + } else { + log_printf("[cmd] usage: pid \r\n"); + } + } else { + log_printf("[cmd] unknown: %s\r\n", cmd_buf); } } } diff --git a/app/control_task.c b/app/control_task.c index 4acfa0b..51bf324 100644 --- a/app/control_task.c +++ b/app/control_task.c @@ -12,6 +12,7 @@ #include "app.h" #include "cmsis_os2.h" +#include "dead_zone.h" #include "mf4010v2.h" #include "pid.h" #include @@ -27,8 +28,8 @@ static float g_target_pitch = 0.0f; /* Pitch 目标 (deg) */ static float g_target_yaw = 0.0f; /* Yaw 目标 (deg) */ /* 电机实例 */ -static mf4010v2_t motor_yaw; /* Yaw 轴电机（外框，补偿 Roll） */ -static mf4010v2_t motor_pitch; /* Pitch 轴电机（内框，补偿 Pitch） */ +static mf4010v2_t motor_1; /* Yaw 轴电机（外框，补偿 Roll） */ +static mf4010v2_t motor_2; /* Pitch 轴电机（内框，补偿 Pitch） */ /* PID 控制器 */ static pid_t g_roll_pid; /* Roll 轴 PID */ @@ -36,6 +37,7 @@ static pid_t g_pitch_pid; /* Pitch 轴 PID */ static lowpass_t g_roll_lowpass; static lowpass_t g_pitch_lowpass; +static dead_zone_t g_yaw_deadzone; /* 电机使能状态 — 由 callback_task 通过 UART 命令控制 */ static bool g_motor_enabled[2] = {false, false}; @@ -63,20 +65,19 @@ static bool g_motor_enabled[2] = {false, false}; * Kd: 抑制振荡的微分系数 * 例如 Kd=50, 则角速度变化率反馈阻尼 */ -#define ROLL_KP 200.0f -#define ROLL_KI 5.0f -#define ROLL_KD 50.0f +#define ROLL_KP 50.0f +#define ROLL_KI 0.0f +#define ROLL_KD 0.0f #define ROLL_ALPHA 0.7f -#define PITCH_KP 200.0f -#define PITCH_KI 5.0f -#define PITCH_KD 50.0f +#define PITCH_KP 50.0f +#define PITCH_KI 0.0f +#define PITCH_KD 0.0f #define PITCH_ALPHA 0.7f /* ============================================================================ * 控制任务（速度控制模式） * ============================================================================ */ - void control_task(void) { imu_data_t imu; @@ -102,29 +103,30 @@ void control_task(void) lowpass_init(&g_roll_lowpass, ROLL_ALPHA); lowpass_init(&g_pitch_lowpass, PITCH_ALPHA); + dead_zone_init(&g_yaw_deadzone, 0.2f); log_printf("[control_task] PID: Roll(Kp=%.1f Ki=%.1f Kd=%.1f) Pitch(Kp=%.1f Ki=%.1f Kd=%.1f)\n", ROLL_KP, ROLL_KI, ROLL_KD, PITCH_KP, PITCH_KI, PITCH_KD); /* 2. 初始化电机 */ - mf4010v2_init(&motor_yaw, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */ - mf4010v2_init(&motor_pitch, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */ + mf4010v2_init(&motor_1, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */ + mf4010v2_init(&motor_2, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */ /* 3. 电机关机状态启动 */ - mf4010v2_close(&motor_yaw); - mf4010v2_close(&motor_pitch); + mf4010v2_close(&motor_1); + mf4010v2_close(&motor_2); - log_printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_yaw)); - log_printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_pitch)); + log_printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_1)); + log_printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_2)); /* 等待 IMU 稳定 */ osDelay(pdMS_TO_TICKS(5000)); /* 4. 使能电机（零速启动） */ - mf4010v2_set_speed(&motor_yaw, 0); - mf4010v2_set_speed(&motor_pitch, 0); - mf4010v2_run(&motor_yaw); - mf4010v2_run(&motor_pitch); + mf4010v2_set_speed(&motor_1, 0); + mf4010v2_set_speed(&motor_2, 0); + mf4010v2_run(&motor_1); + mf4010v2_run(&motor_2); log_printf("[control_task] stabilization enabled (speed mode)\n"); @@ -144,30 +146,40 @@ void control_task(void) if (qstat == osOK) { imu.angle[0] = lowpass_update(&g_roll_lowpass, imu.angle[0]); imu.angle[1] = lowpass_update(&g_pitch_lowpass, imu.angle[1]); - + imu.angle[0] = dead_zone_apply(&g_yaw_deadzone, imu.angle[0]); /* ---- Roll 轴速度控制 ---- */ float roll_speed = pid_compute(&g_roll_pid, g_target_roll, imu.angle[0]); /* Roll */ - int32_t yaw_speed_cmd = (int32_t)roll_speed; + int32_t motor_1_speed_cmd = (int32_t)roll_speed; /* ---- Pitch 轴速度控制 ---- */ float pitch_speed = pid_compute(&g_pitch_pid, g_target_pitch, imu.angle[1]); /* Pitch */ - int32_t pitch_speed_cmd = (int32_t)pitch_speed; - yaw_speed_cmd = yaw_speed_cmd > 36000 ? 36000 : (yaw_speed_cmd < -36000 ? -36000 : yaw_speed_cmd); - pitch_speed_cmd = pitch_speed_cmd > 36000 ? 36000 : (pitch_speed_cmd < -36000 ? -36000 : pitch_speed_cmd); + int32_t motor_2_speed_cmd = (int32_t)pitch_speed; + motor_1_speed_cmd = motor_1_speed_cmd > 36000 ? 36000 : (motor_1_speed_cmd < -36000 ? -36000 : motor_1_speed_cmd); + motor_2_speed_cmd = motor_2_speed_cmd > 36000 ? 36000 : (motor_2_speed_cmd < -36000 ? -36000 : motor_2_speed_cmd); if (g_motor_enabled[GIMBAL_MOTOR_YAW]) { - mf4010v2_set_speed(&motor_yaw, yaw_speed_cmd); + if (imu.angle[0] < 1e-6f && imu.angle[0] > -1e-6f) { + mf4010v2_set_speed(&motor_1, 0); + log_printf("[control_task] motor 1: %d,", 0); + pid_reset(&g_roll_pid); + } else { + mf4010v2_set_speed(&motor_1, motor_1_speed_cmd); + log_printf("[control_task] motor 1: %ld,", motor_1_speed_cmd); + } } else { - mf4010v2_close(&motor_yaw); + log_printf("[control_task] motor 1: %d,", 0); + mf4010v2_close(&motor_1); } if (g_motor_enabled[GIMBAL_MOTOR_PITCH]) { - mf4010v2_set_speed(&motor_pitch, pitch_speed_cmd); + mf4010v2_set_speed(&motor_2, motor_2_speed_cmd); + log_printf("motor 2: %ld\r\n", motor_2_speed_cmd); } else { - mf4010v2_close(&motor_pitch); + mf4010v2_close(&motor_2); + log_printf(" motor 2: %d\r\n", 0); } } @@ -212,7 +224,7 @@ void gimbal_motor_disable(void) void gimbal_motor_enable_id(int id) { g_motor_enabled[id] = true; - mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_yaw : &motor_pitch; + mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_1 : &motor_2; pid_t *pid = (id == GIMBAL_MOTOR_YAW) ? &g_roll_pid : &g_pitch_pid; mf4010v2_set_speed(motor, 0); mf4010v2_run(motor); @@ -223,7 +235,7 @@ void gimbal_motor_enable_id(int id) void gimbal_motor_disable_id(int id) { g_motor_enabled[id] = false; - mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_yaw : &motor_pitch; + mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_1 : &motor_2; mf4010v2_close(motor); } diff --git a/app/sensor_task.c b/app/sensor_task.c index 6c99ca2..babe2ee 100644 --- a/app/sensor_task.c +++ b/app/sensor_task.c @@ -152,7 +152,7 @@ void sensor_task(void) } /* 将 IMU 数据推送到 control_task */ if (g_imu_queue != NULL) { - osMessageQueuePut(g_imu_queue, &imu, 0, 20); + osMessageQueuePut(g_imu_queue, &imu, 0, 5); } } if (osDelayUntil(last_tick) != osOK) { diff --git a/modules/filter/dead_zone/dead_zone.c b/modules/filter/dead_zone/dead_zone.c new file mode 100644 index 0000000..edc1d47 --- /dev/null +++ b/modules/filter/dead_zone/dead_zone.c @@ -0,0 +1,25 @@ +#include "dead_zone.h" + +void dead_zone_init(dead_zone_t *dz, float threshold) +{ + if (!dz) return; + dz->threshold = (threshold < 0.0f) ? 0.0f : threshold; +} + +float dead_zone_apply(dead_zone_t *dz, float x) +{ + if (!dz) return x; + + if (x > dz->threshold) + return x - dz->threshold; + else if (x < -dz->threshold) + return x + dz->threshold; + else + return 0.0f; +} + +void dead_zone_set(dead_zone_t *dz, float threshold) +{ + if (!dz) return; + dz->threshold = (threshold < 0.0f) ? 0.0f : threshold; +} diff --git a/modules/filter/dead_zone/dead_zone.h b/modules/filter/dead_zone/dead_zone.h new file mode 100644 index 0000000..e05c4ac --- /dev/null +++ b/modules/filter/dead_zone/dead_zone.h @@ -0,0 +1,20 @@ +#ifndef DEAD_ZONE_H +#define DEAD_ZONE_H + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + float threshold; +} dead_zone_t; + +void dead_zone_init(dead_zone_t *dz, float threshold); +float dead_zone_apply(dead_zone_t *dz, float x); +void dead_zone_set(dead_zone_t *dz, float threshold); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/release b/release index 07f1a2e..2483e67 160000 --- a/release +++ b/release @@ -1 +1 @@ -Subproject commit 07f1a2efda7b3fd6e6ee4236136a487086cdcbe1 +Subproject commit 2483e67f90a995b65c4897a58e7c17d3e1906efc