From 361caca42753da836790acabf5b87d5c55af2c29 Mon Sep 17 00:00:00 2001 From: robinson Date: Sat, 23 May 2026 00:16:40 +0800 Subject: [PATCH] [release] version 1.0.0 --- app/callback_task.c | 55 +++++++++++---------- app/control_task.c | 84 +++++++++++++++++++++++++-------- app/sensor_task.c | 2 +- modules/control/pid/pid.c | 2 - modules/device/motor/mf4010v2.c | 68 +++++++++++++++++++++++--- modules/device/motor/mf4010v2.h | 9 +++- release | 2 +- 7 files changed, 164 insertions(+), 58 deletions(-) diff --git a/app/callback_task.c b/app/callback_task.c index 3625414..ebc643c 100644 --- a/app/callback_task.c +++ b/app/callback_task.c @@ -20,11 +20,11 @@ static struct { /* * 支持的命令（格式：@command\n）: * "motor enable" — 使能全部电机（零速启动） - * "motor enable yaw" — 使能 Yaw 电机 - * "motor enable pitch" — 使能 Pitch 电机 + * "motor enable 1" — 使能电机 1 + * "motor enable 2" — 使能电机 2 * "motor disable" — 关闭全部电机（惰行 coast） - * "motor disable yaw" — 关闭 Yaw 电机 - * "motor disable pitch" — 关闭 Pitch 电机 + * "motor disable 1" — 关闭电机 1 + * "motor disable 2" — 关闭电机 2 * "sensor enable" — 恢复 IMU 读取 * "sensor disable" — 暂停 IMU 读取 * "sensorcali acc enable" — 开始加速度计标定 @@ -98,8 +98,8 @@ void callback_task(void) 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" + " motor enable [1|2] ==> enable motor(s), default both\r\n" + " motor disable [1|2] ==> 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" @@ -107,20 +107,20 @@ void callback_task(void) " 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"); + " pid

==> set PID param (v÷10=实际值, motor disabled)\r\n" + " motor: 1|2, 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) { + } else if (strcmp(p, "1") == 0) { gimbal_motor_enable_id(GIMBAL_MOTOR_YAW); - log_printf("[cmd] yaw motor enabled\r\n"); - } else if (strcmp(p, "pitch") == 0) { + log_printf("[cmd] motor 1 enabled\r\n"); + } else if (strcmp(p, "2") == 0) { gimbal_motor_enable_id(GIMBAL_MOTOR_PITCH); - log_printf("[cmd] pitch motor enabled\r\n"); + log_printf("[cmd] motor 2 enabled\r\n"); } else { log_printf("[cmd] unknown motor: %s\r\n", p); } @@ -130,12 +130,12 @@ void callback_task(void) if (*p == '\0') { gimbal_motor_disable(); log_printf("[cmd] both motors disabled\r\n"); - } else if (strcmp(p, "yaw") == 0) { + } else if (strcmp(p, "1") == 0) { gimbal_motor_disable_id(GIMBAL_MOTOR_YAW); - log_printf("[cmd] yaw motor disabled\r\n"); - } else if (strcmp(p, "pitch") == 0) { + log_printf("[cmd] motor 1 disabled\r\n"); + } else if (strcmp(p, "2") == 0) { gimbal_motor_disable_id(GIMBAL_MOTOR_PITCH); - log_printf("[cmd] pitch motor disabled\r\n"); + log_printf("[cmd] motor 2 disabled\r\n"); } else { log_printf("[cmd] unknown motor: %s\r\n", p); } @@ -161,28 +161,31 @@ void callback_task(void) /* 查看当前 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", + log_printf("[cmd] Motor 1: 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", + log_printf("[cmd] Motor 2: 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()) { + /* 调参：pid <1|2> （value ÷ 10 = 实际系数） */ + char param[8]; + int motor_id, int_val; + if (sscanf(cmd_buf, "pid %d %7s %d", &motor_id, param, &int_val) == 3) { + if (motor_id < 1 || motor_id > 2) { + log_printf("[cmd] usage: pid <1|2> \r\n"); + } else if (gimbal_is_motor_enabled()) { log_printf("[cmd] ERROR: disable motor first (motor disable)\r\n"); } else { + const char *axis = (motor_id == 1) ? "roll" : "pitch"; 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); + log_printf("[cmd] motor %d.%s = %d\r\n", motor_id, param, int_val); } else { - log_printf("[cmd] usage: pid \r\n"); + log_printf("[cmd] usage: pid <1|2> \r\n"); } } } else { - log_printf("[cmd] usage: pid \r\n"); + log_printf("[cmd] usage: pid <1|2> \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 51bf324..0c02a9d 100644 --- a/app/control_task.c +++ b/app/control_task.c @@ -15,6 +15,7 @@ #include "dead_zone.h" #include "mf4010v2.h" #include "pid.h" +#include #include #include #include "lowpass.h" @@ -51,7 +52,7 @@ static bool g_motor_enabled[2] = {false, false}; * 电机 MF4010V2 速度范围：±204800 (即 ±2048 DPS) * 稳像场景使用较低速度：建议 ±36000 (即 ±360 DPS) 作为输出上限 */ -#define SPEED_OUTPUT_MAX 36000.0f /* 最大角速度 360 DPS (0.01 DPS) */ +#define SPEED_OUTPUT_MAX 204800.0f /* 最大角速度 360 DPS (0.01 DPS) */ /* * PID 参数说明（速度控制模式）: @@ -65,19 +66,66 @@ static bool g_motor_enabled[2] = {false, false}; * Kd: 抑制振荡的微分系数 * 例如 Kd=50, 则角速度变化率反馈阻尼 */ -#define ROLL_KP 50.0f +#define ROLL_KP 1020.0f #define ROLL_KI 0.0f -#define ROLL_KD 0.0f +#define ROLL_KD 20.0f #define ROLL_ALPHA 0.7f -#define PITCH_KP 50.0f +#define PITCH_KP 1020.0f #define PITCH_KI 0.0f -#define PITCH_KD 0.0f +#define PITCH_KD 20.0f #define PITCH_ALPHA 0.7f /* ============================================================================ * 控制任务（速度控制模式） * ============================================================================ */ + +// void control_task(void) { +// log_printf("[control_task] starting stabilization control (speed mode)\n"); +// // mf4010v2_init(&motor_1, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */ +// mf4010v2_init(&motor_2, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */ +// // mf4010v2_close(&motor_1); +// mf4010v2_close(&motor_2); +// osDelay(pdMS_TO_TICKS(5000)); +// // mf4010v2_run(&motor_1); +// mf4010v2_run(&motor_2); +// // mf4010v2_set_speed(&motor_1, 0); +// mf4010v2_set_speed(&motor_2, 0); +// // int8_t tempture; +// int16_t speed; +// int ret; +// int aim_speed; +// while(true) { +// aim_speed = 500; +// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL); +// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret); +// ret = mf4010v2_set_speed(&motor_2, aim_speed); +// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret); +// osDelay(pdMS_TO_TICKS(2000)); + +// aim_speed = 1000; +// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL); +// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret); +// ret = mf4010v2_set_speed(&motor_2, aim_speed); +// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret); +// osDelay(pdMS_TO_TICKS(2000)); + +// aim_speed = -1000; +// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL); +// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret); +// ret = mf4010v2_set_speed(&motor_2, aim_speed); +// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret); +// osDelay(pdMS_TO_TICKS(2000)); + +// aim_speed = -500; +// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL); +// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret); +// ret = mf4010v2_set_speed(&motor_2, aim_speed); +// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret); +// osDelay(pdMS_TO_TICKS(2000)); + +// } +// } void control_task(void) { imu_data_t imu; @@ -139,7 +187,7 @@ void control_task(void) * 3. 速度指令发送到电机（电机内部完成速度闭环） * ======================================================================== */ while (1) { - last_tick += pdMS_TO_TICKS(20); /* 100Hz 周期 */ + last_tick += pdMS_TO_TICKS(20); /* 50Hz 周期 */ /* 从 sensor_task 获取最新 IMU 数据（不等待，由定时保证节奏） */ qstat = osMessageQueueGet(g_imu_queue, &imu, NULL, 0); @@ -158,28 +206,26 @@ void control_task(void) g_target_pitch, imu.angle[1]); /* Pitch */ 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); - + log_printf("[control_task:motor] %d, %d", (int)(imu.angle[0]*10), (int)(imu.angle[1]*10)); if (g_motor_enabled[GIMBAL_MOTOR_YAW]) { 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); + mf4010v2_set_speed_nb(&motor_1, 0); + log_printf(" 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); + mf4010v2_set_speed_nb(&motor_1, motor_1_speed_cmd); + log_printf(" 1: %ld,", motor_1_speed_cmd); } } else { - log_printf("[control_task] motor 1: %d,", 0); - mf4010v2_close(&motor_1); + log_printf(" 1: %d,", 0); + mf4010v2_close_nb(&motor_1); } if (g_motor_enabled[GIMBAL_MOTOR_PITCH]) { - mf4010v2_set_speed(&motor_2, motor_2_speed_cmd); - log_printf("motor 2: %ld\r\n", motor_2_speed_cmd); + mf4010v2_set_speed_nb(&motor_2, motor_2_speed_cmd); + log_printf(" 2: %ld\r\n", motor_2_speed_cmd); } else { - mf4010v2_close(&motor_2); - log_printf(" motor 2: %d\r\n", 0); + mf4010v2_close_nb(&motor_2); + log_printf(" 2: %d\r\n", 0); } } diff --git a/app/sensor_task.c b/app/sensor_task.c index babe2ee..9d9f5b7 100644 --- a/app/sensor_task.c +++ b/app/sensor_task.c @@ -116,7 +116,7 @@ void sensor_task(void) if(g_sensor_enabled) { WitReadReg(AX, 13); } - last_tick += pdMS_TO_TICKS(2); /* 500Hz 轮询 */ + last_tick += pdMS_TO_TICKS(5); /* 500Hz 轮询 */ if(g_sensor_enabled && s_cDataUpdate) { for(i = 0; i < 3; i++) diff --git a/modules/control/pid/pid.c b/modules/control/pid/pid.c index ad16455..93aa66e 100644 --- a/modules/control/pid/pid.c +++ b/modules/control/pid/pid.c @@ -15,7 +15,6 @@ void pid_init(pid_t *pid) float pid_compute(pid_t *pid, float target, float actual) { if (!pid) return 0.0f; - float error = target - actual; /* 比例项 */ @@ -34,7 +33,6 @@ float pid_compute(pid_t *pid, float target, float actual) pid->integral = -pid->integral_max; } float i_term = pid->Ki * pid->integral; - float output = p_term + i_term + d_term; /* 输出限幅（0=不限幅） */ diff --git a/modules/device/motor/mf4010v2.c b/modules/device/motor/mf4010v2.c index efaf4b6..27f2703 100644 --- a/modules/device/motor/mf4010v2.c +++ b/modules/device/motor/mf4010v2.c @@ -63,10 +63,8 @@ int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8]) int ret = can_if_send(motor->can_ch, &msg); if (ret != 0) { motor->error_count++; - // printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id); return MF4010_ERR_CAN_SEND; } - /* 轮询等待电机响应（带超时） */ for (int retry = 0; retry < MF4010_RECV_RETRIES; retry++) { ret = can_if_recv(motor->can_ch, &msg); @@ -81,8 +79,10 @@ int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8]) printf("Error: Timeout waiting for motor response (ID 0x%03X)\r\n", motor->id); return MF4010_ERR_NO_RESPONSE; } - memcpy(motor->last_response, msg.data, 8); + if (motor->last_response[0] != command[0]) { + return MF4010_ERR_BAD_RECV; + } motor->success_count++; return MF4010_OK; } @@ -168,6 +168,41 @@ void mf4010v2_close(mf4010v2_t* motor) { } } +// int32_t mf4010v2_get_speed(mf4010v2_t* motor) { +// can_message_t msg; +// if (!motor) return MF4010_ERR_INVALID_PARAM; +// msg.id = motor->id; +// uint8_t commands = [0x9c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]; +// mf4010v2_send_command(motor, commands); +// uint8_t = motor->last_response +// } +/* ============================================================================ + * 非阻塞发送 — fire-and-forget，不轮询 CAN 响应 + * ============================================================================ */ +static int send_cmd_nb(mf4010v2_t* motor, const uint8_t command[8]) +{ + can_message_t msg; + if (!motor) return MF4010_ERR_INVALID_PARAM; + msg.id = motor->id; + memcpy(msg.data, command, 8); + msg.length = 8; + return can_if_send(motor->can_ch, &msg); +} + +int mf4010v2_set_speed_nb(mf4010v2_t* motor, int32_t speed_0_01dps) +{ + if (!motor) return MF4010_ERR_INVALID_PARAM; + uint8_t cmd[8]; + cmd_speed_control(cmd, speed_0_01dps); + return send_cmd_nb(motor, cmd); +} + +int mf4010v2_close_nb(mf4010v2_t* motor) +{ + if (!motor) return MF4010_ERR_INVALID_PARAM; + uint8_t cmd[8] = COMMAND_MOTOR_CLOSE; + return send_cmd_nb(motor, cmd); +} int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) { if (!motor) return MF4010_ERR_INVALID_PARAM; @@ -205,10 +240,10 @@ void cmd_speed_control(uint8_t* buf, int32_t speed) buf[1] = 0x00; buf[2] = 0x00; buf[3] = 0x00; - buf[4] = (uint8_t)(speed & 0xFF); - buf[5] = (uint8_t)((speed >> 8) & 0xFF); - buf[6] = (uint8_t)((speed >> 16) & 0xFF); - buf[7] = (uint8_t)((speed >> 24) & 0xFF); + buf[4] = *(uint8_t *)(&speed); + buf[5] = *((uint8_t *)(&speed)+1); + buf[6] = *((uint8_t *)(&speed)+2); + buf[7] = *((uint8_t *)(&speed)+3); } @@ -337,3 +372,22 @@ uint8_t mf4010v2_get_error_code(mf4010v2_t* motor) { return (motor != NULL) ? (motor->last_response[0] & 0x7F) : 0; } + +int mf4010v2_get_motor_state(mf4010v2_t* motor, int8_t* tempture, int16_t* iq, int16_t* speed, int16_t* encoder) { + uint8_t command[] = {0x9c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + mf4010v2_send_command(motor, command); + if (motor->last_response[0] != 0x9c) { + motor->flags |= MF4010_FLAG_FAULT; + return -1; + } + motor->flags &= ~MF4010_FLAG_FAULT; + if (tempture != NULL) + *tempture = *(int8_t*)(motor->last_response + 1); + if (iq != NULL) + *iq = (int16_t)(motor->last_response[2] | (motor->last_response[3] << 8)); + if(speed != NULL) + *speed = (int16_t)(motor->last_response[4] | (motor->last_response[5] << 8)); + if (encoder != NULL) + *encoder = (int16_t)(motor->last_response[6] | (motor->last_response[7] << 8)); + return 0; +} \ No newline at end of file diff --git a/modules/device/motor/mf4010v2.h b/modules/device/motor/mf4010v2.h index eba598c..0d66df7 100644 --- a/modules/device/motor/mf4010v2.h +++ b/modules/device/motor/mf4010v2.h @@ -41,7 +41,8 @@ typedef enum { MF4010_ERR_CRC = -6, /**< CRC 校验错误 */ MF4010_ERR_HARDWARE = -7, /**< 硬件错误 */ MF4010_ERR_CAN_SEND = -8, /**< CAN 发送失败 */ - MF4010_ERR_CAN_RECV = -9 /**< CAN 接收失败 */ + MF4010_ERR_CAN_RECV = -9, /**< CAN 接收失败 */ + MF4010_ERR_BAD_RECV = -10 } mf4010_status_t; /** @@ -115,7 +116,11 @@ void mf4010v2_close(mf4010v2_t* motor); int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps); int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg); int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid); -int32_t mf4010v2_get_speed(mf4010v2_t* motor); +int mf4010v2_get_motor_state(mf4010v2_t* motor, int8_t* tempture, int16_t* iq, int16_t* speed, int16_t* encoder); + +/* Non-blocking variants — fire-and-forget, no CAN response polling */ +int mf4010v2_set_speed_nb(mf4010v2_t* motor, int32_t speed_0_01dps); +int mf4010v2_close_nb(mf4010v2_t* motor); /* ============================================================================ * 命令宏定义（8 字节 CAN 命令帧） diff --git a/release b/release index 2483e67..86fdbcb 160000 --- a/release +++ b/release @@ -1 +1 @@ -Subproject commit 2483e67f90a995b65c4897a58e7c17d3e1906efc +Subproject commit 86fdbcb85a7d2ab8704a426032f22503d0e2b6ae