From c8700aa3530670fd95dce335f36d03a71cab2179 Mon Sep 17 00:00:00 2001 From: yelvlab Date: Tue, 28 Jul 2026 15:29:14 +0800 Subject: [PATCH] =?UTF-8?q?add(cmd):=E6=9E=84=E5=BB=BA=E6=8E=A8=E6=9D=86?= =?UTF-8?q?=E7=9A=84=E5=91=BD=E4=BB=A4M3=E6=8E=A8=E5=87=BA=EF=BC=8CM4?= =?UTF-8?q?=E6=8B=89=E5=9B=9E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Src/command.c | 45 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/Src/command.c b/Src/command.c index bdfeb46..8ee2752 100644 --- a/Src/command.c +++ b/Src/command.c @@ -445,6 +445,51 @@ void handle_command(const uint8_t *frame, uint8_t len) { send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok)); return; + case 3u: // M3 p_val: Actuator Channel. s_val: Actuator Duty Cycle + { + uint32_t p_val = 0; + uint32_t s_val = 0; + + bool has_p = find_parameter_value(cmd, cmd_len, 'P', &p_val); + bool has_s = find_parameter_value(cmd, cmd_len, 'S', &s_val); + + if (!(has_p && has_s)) { + send_response(RESP_TYPE_PARAM_ERR, s_report_status_err, sizeof(s_report_status_err)); + return; + } + + if (p_val > (uint32_t)ACTUATOR_CH2 || s_val > 999u) { + send_response(RESP_TYPE_PARAM_ERR, s_report_status_err, sizeof(s_report_status_err)); + return; + } + COMMAND_DEBUG("M3 Command(PUSH): Actuator Channel=%u, Duty Cycle=%u", p_val, s_val); + actuator_output((actuator_channel_t)p_val, (uint16_t)s_val, DIRECTION_PUSH); + send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok)); + } + return; + case 4u: // M4 p_val: Actuator Channel. s_val: Actuator Duty Cycle + { + uint32_t p_val = 0; + uint32_t s_val = 0; + + bool has_p = find_parameter_value(cmd, cmd_len, 'P', &p_val); + bool has_s = find_parameter_value(cmd, cmd_len, 'S', &s_val); + + if (!(has_p && has_s)) { + send_response(RESP_TYPE_PARAM_ERR, s_report_status_err, sizeof(s_report_status_err)); + return; + } + + if (p_val > (uint32_t)ACTUATOR_CH2 || s_val > 999u) { + send_response(RESP_TYPE_PARAM_ERR, s_report_status_err, sizeof(s_report_status_err)); + return; + } + COMMAND_DEBUG("M4 Command(PULL): Actuator Channel=%u, Duty Cycle=%u", p_val, s_val); + actuator_output((actuator_channel_t)p_val, (uint16_t)s_val, DIRECTION_PULL); + send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok)); + } + return; + /* ========================================== * M888 软件重启命令 * ========================================== */