ArmDev/motor-controller
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[feat] project add deadzone

Browse Source
This commit is contained in:
robinson
2026-05-22 00:12:16 +08:00
parent bbe8bd06bc
commit 9171374490
10 changed files with 188 additions and 202 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -51,6 +51,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
modules/filter/moving_average/moving_average.c modules/filter/moving_average/moving_average.c
modules/filter/notch/notch.c modules/filter/notch/notch.c
modules/filter/rate_limiter/rate_limiter.c modules/filter/rate_limiter/rate_limiter.c
modules/filter/dead_zone/dead_zone.c
modules/device/motor/mf4010v2.c modules/device/motor/mf4010v2.c
modules/device/led/led.c modules/device/led/led.c
modules/device/can/can_device.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/moving_average
${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/notch ${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/notch
${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/rate_limiter ${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/control/pid
${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/soft_i2c ${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/soft_i2c
${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/i2c ${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/i2c

2
CloudPlant.ioc
View File

@@ -19,7 +19,7 @@ CAN2.NART=ENABLE
CAN2.Prescaler=6 CAN2.Prescaler=6
FREERTOS.FootprintOK=true FREERTOS.FootprintOK=true
FREERTOS.IPParameters=Tasks01,configENABLE_FPU,configUSE_NEWLIB_REENTRANT,configMINIMAL_STACK_SIZE,configTOTAL_HEAP_SIZE,FootprintOK 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.configENABLE_FPU=1
FREERTOS.configMINIMAL_STACK_SIZE=256 FREERTOS.configMINIMAL_STACK_SIZE=256
FREERTOS.configTOTAL_HEAP_SIZE=32768 FREERTOS.configTOTAL_HEAP_SIZE=32768

4
Core/Src/freertos.c
View File

@@ -59,14 +59,14 @@ osThreadId_t controlTaskHandle;
const osThreadAttr_t controlTask_attributes = { const osThreadAttr_t controlTask_attributes = {
.name = "controlTask", .name = "controlTask",
.stack_size = 256 * 4, .stack_size = 256 * 4,
.priority = (osPriority_t) osPriorityNormal, .priority = (osPriority_t) osPriorityHigh,
}; };
/* Definitions for sensorTask */ /* Definitions for sensorTask */
osThreadId_t sensorTaskHandle; osThreadId_t sensorTaskHandle;
const osThreadAttr_t sensorTask_attributes = { const osThreadAttr_t sensorTask_attributes = {
.name = "sensorTask", .name = "sensorTask",
.stack_size = 256 * 4, .stack_size = 256 * 4,
.priority = (osPriority_t) osPriorityHigh, .priority = (osPriority_t) osPriorityNormal,
}; };
/* Private function prototypes -----------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/

2
Core/Src/main.c
View File

@@ -21,10 +21,8 @@
#include "cmsis_os.h" #include "cmsis_os.h"
#include "can.h" #include "can.h"
#include "i2c.h" #include "i2c.h"
#include "stm32f4xx_hal_uart.h"
#include "usart.h" #include "usart.h"
#include "gpio.h" #include "gpio.h"
#include <stdint.h>
/* Private includes ----------------------------------------------------------*/ /* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */ /* USER CODE BEGIN Includes */

259
app/callback_task.c
View File

@@ -1,8 +1,6 @@
#include "app.h" #include "app.h"
#include "cmd_parser.h"
#include "cmsis_os2.h" #include "cmsis_os2.h"
#include <string.h> #include <string.h>
#include <stdlib.h>
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "queue.h" #include "queue.h"
#include "stm32f4xx_hal.h" #include "stm32f4xx_hal.h"
@@ -84,179 +82,110 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
portYIELD_FROM_ISR(xHigherPriorityTaskWoken); 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 <axis> <p> <v> ==> 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 <roll|pitch> <kp|ki|kd> <value>\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 <roll|pitch> <kp|ki|kd> <value>\r\n");
}
return 0;
}
return -1;
}
void callback_task(void) void callback_task(void)
{ {
char cmd_buf[MAX_CMD_LEN]; 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.state = USER_CMD_IDLE;
g_cmd_parser.index = 0; g_cmd_parser.index = 0;
g_cmd_parser.cmd_queue = g_cmd_queue; 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) { while (1) {
osStatus_t qstat = osMessageQueueGet(g_cmd_queue, cmd_buf, NULL, osWaitForever); osStatus_t qstat = osMessageQueueGet(g_cmd_queue, cmd_buf, NULL, osWaitForever);
if (qstat != osOK) continue; if (qstat != osOK) {
if (cmd_buf[0] == '\0') continue; continue;
}
memcpy(disp_buf, cmd_buf, MAX_CMD_LEN); if (strcmp(cmd_buf, "help") == 0) {
if (cmd_parser_dispatch(&parser, cmd_buf, NULL) != 0) { log_printf("[cmd] available commands:\r\n"
log_printf("[cmd] unknown: %s\r\n", disp_buf); " 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 <axis> <p> <v> ==> 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 <roll|pitch> <kp|ki|kd> <value> （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 <roll|pitch> <kp|ki|kd> <value>\r\n");
}
}
} else {
log_printf("[cmd] usage: pid <roll|pitch> <kp|ki|kd> <value>\r\n");
}
} else {
log_printf("[cmd] unknown: %s\r\n", cmd_buf);
} }
} }
} }

72
app/control_task.c
View File

@@ -12,6 +12,7 @@
#include "app.h" #include "app.h"
#include "cmsis_os2.h" #include "cmsis_os2.h"
#include "dead_zone.h"
#include "mf4010v2.h" #include "mf4010v2.h"
#include "pid.h" #include "pid.h"
#include <stdio.h> #include <stdio.h>
@@ -27,8 +28,8 @@ static float g_target_pitch = 0.0f; /* Pitch 目标 (deg) */
static float g_target_yaw = 0.0f; /* Yaw 目标 (deg) */ static float g_target_yaw = 0.0f; /* Yaw 目标 (deg) */
/* 电机实例 */ /* 电机实例 */
static mf4010v2_t motor_yaw; /* Yaw 轴电机（外框，补偿 Roll） */ static mf4010v2_t motor_1; /* Yaw 轴电机（外框，补偿 Roll） */
static mf4010v2_t motor_pitch; /* Pitch 轴电机（内框，补偿 Pitch） */ static mf4010v2_t motor_2; /* Pitch 轴电机（内框，补偿 Pitch） */
/* PID 控制器 */ /* PID 控制器 */
static pid_t g_roll_pid; /* Roll 轴 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_roll_lowpass;
static lowpass_t g_pitch_lowpass; static lowpass_t g_pitch_lowpass;
static dead_zone_t g_yaw_deadzone;
/* 电机使能状态 — 由 callback_task 通过 UART 命令控制 */ /* 电机使能状态 — 由 callback_task 通过 UART 命令控制 */
static bool g_motor_enabled[2] = {false, false}; static bool g_motor_enabled[2] = {false, false};
@@ -63,20 +65,19 @@ static bool g_motor_enabled[2] = {false, false};
* Kd: 抑制振荡的微分系数 * Kd: 抑制振荡的微分系数
* 例如 Kd=50, 则角速度变化率反馈阻尼 * 例如 Kd=50, 则角速度变化率反馈阻尼
*/ */
#define ROLL_KP 200.0f #define ROLL_KP 50.0f
#define ROLL_KI 5.0f #define ROLL_KI 0.0f
#define ROLL_KD 50.0f #define ROLL_KD 0.0f
#define ROLL_ALPHA 0.7f #define ROLL_ALPHA 0.7f
#define PITCH_KP 200.0f #define PITCH_KP 50.0f
#define PITCH_KI 5.0f #define PITCH_KI 0.0f
#define PITCH_KD 50.0f #define PITCH_KD 0.0f
#define PITCH_ALPHA 0.7f #define PITCH_ALPHA 0.7f
/* ============================================================================ /* ============================================================================
* 控制任务（速度控制模式） * 控制任务（速度控制模式）
* ============================================================================ */ * ============================================================================ */
void control_task(void) void control_task(void)
{ {
imu_data_t imu; imu_data_t imu;
@@ -102,29 +103,30 @@ void control_task(void)
lowpass_init(&g_roll_lowpass, ROLL_ALPHA); lowpass_init(&g_roll_lowpass, ROLL_ALPHA);
lowpass_init(&g_pitch_lowpass, PITCH_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", 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); ROLL_KP, ROLL_KI, ROLL_KD, PITCH_KP, PITCH_KI, PITCH_KD);
/* 2. 初始化电机 */ /* 2. 初始化电机 */
mf4010v2_init(&motor_yaw, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */ mf4010v2_init(&motor_1, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */
mf4010v2_init(&motor_pitch, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */ mf4010v2_init(&motor_2, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */
/* 3. 电机关机状态启动 */ /* 3. 电机关机状态启动 */
mf4010v2_close(&motor_yaw); mf4010v2_close(&motor_1);
mf4010v2_close(&motor_pitch); mf4010v2_close(&motor_2);
log_printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_yaw)); 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_pitch)); log_printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_2));
/* 等待 IMU 稳定 */ /* 等待 IMU 稳定 */
osDelay(pdMS_TO_TICKS(5000)); osDelay(pdMS_TO_TICKS(5000));
/* 4. 使能电机（零速启动） */ /* 4. 使能电机（零速启动） */
mf4010v2_set_speed(&motor_yaw, 0); mf4010v2_set_speed(&motor_1, 0);
mf4010v2_set_speed(&motor_pitch, 0); mf4010v2_set_speed(&motor_2, 0);
mf4010v2_run(&motor_yaw); mf4010v2_run(&motor_1);
mf4010v2_run(&motor_pitch); mf4010v2_run(&motor_2);
log_printf("[control_task] stabilization enabled (speed mode)\n"); log_printf("[control_task] stabilization enabled (speed mode)\n");
@@ -144,30 +146,40 @@ void control_task(void)
if (qstat == osOK) { if (qstat == osOK) {
imu.angle[0] = lowpass_update(&g_roll_lowpass, imu.angle[0]); imu.angle[0] = lowpass_update(&g_roll_lowpass, imu.angle[0]);
imu.angle[1] = lowpass_update(&g_pitch_lowpass, imu.angle[1]); imu.angle[1] = lowpass_update(&g_pitch_lowpass, imu.angle[1]);
imu.angle[0] = dead_zone_apply(&g_yaw_deadzone, imu.angle[0]);
/* ---- Roll 轴速度控制 ---- */ /* ---- Roll 轴速度控制 ---- */
float roll_speed = pid_compute(&g_roll_pid, float roll_speed = pid_compute(&g_roll_pid,
g_target_roll, g_target_roll,
imu.angle[0]); /* 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 轴速度控制 ---- */ /* ---- Pitch 轴速度控制 ---- */
float pitch_speed = pid_compute(&g_pitch_pid, float pitch_speed = pid_compute(&g_pitch_pid,
g_target_pitch, g_target_pitch,
imu.angle[1]); /* Pitch */ imu.angle[1]); /* Pitch */
int32_t pitch_speed_cmd = (int32_t)pitch_speed; int32_t motor_2_speed_cmd = (int32_t)pitch_speed;
yaw_speed_cmd = yaw_speed_cmd > 36000 ? 36000 : (yaw_speed_cmd < -36000 ? -36000 : yaw_speed_cmd); motor_1_speed_cmd = motor_1_speed_cmd > 36000 ? 36000 : (motor_1_speed_cmd < -36000 ? -36000 : motor_1_speed_cmd);
pitch_speed_cmd = pitch_speed_cmd > 36000 ? 36000 : (pitch_speed_cmd < -36000 ? -36000 : pitch_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]) { 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 { } else {
mf4010v2_close(&motor_yaw); log_printf("[control_task] motor 1: %d,", 0);
mf4010v2_close(&motor_1);
} }
if (g_motor_enabled[GIMBAL_MOTOR_PITCH]) { 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 { } 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) void gimbal_motor_enable_id(int id)
{ {
g_motor_enabled[id] = true; 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; pid_t *pid = (id == GIMBAL_MOTOR_YAW) ? &g_roll_pid : &g_pitch_pid;
mf4010v2_set_speed(motor, 0); mf4010v2_set_speed(motor, 0);
mf4010v2_run(motor); mf4010v2_run(motor);
@@ -223,7 +235,7 @@ void gimbal_motor_enable_id(int id)
void gimbal_motor_disable_id(int id) void gimbal_motor_disable_id(int id)
{ {
g_motor_enabled[id] = false; 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); mf4010v2_close(motor);
} }

2
app/sensor_task.c
View File

@@ -152,7 +152,7 @@ void sensor_task(void)
} }
/* 将 IMU 数据推送到 control_task */ /* 将 IMU 数据推送到 control_task */
if (g_imu_queue != NULL) { if (g_imu_queue != NULL) {
osMessageQueuePut(g_imu_queue, &imu, 0, 20); osMessageQueuePut(g_imu_queue, &imu, 0, 5);
} }
} }
if (osDelayUntil(last_tick) != osOK) { if (osDelayUntil(last_tick) != osOK) {

25
modules/filter/dead_zone/dead_zone.c Normal file
View File

@@ -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;
}

20
modules/filter/dead_zone/dead_zone.h Normal file
View File

@@ -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

2
release

Submodule release updated: 07f1a2efda...2483e67f90