diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 611276b..21a8c2a 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -331,7 +331,7 @@ static uint8_t DrivenMotorIsLost() return 0; } -/* Chassis-board local RC control. Right switch down is hard emergency stop. */ +/* Chassis-board local RC control. Rightmost safety switch down is emergency stop. */ static void ControlSwitch() { memset(&chassis_cmd_recv, 0, sizeof(chassis_cmd_recv)); @@ -349,8 +349,13 @@ static void ControlSwitch() } uint8_t switch_right = rc_data[TEMP].rc.switch_right; +#ifdef REMOTE_FS_I6 + if (!switch_is_up(switch_right)) + return; +#else if (!switch_is_up(switch_right) && !switch_is_mid(switch_right)) return; +#endif float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001f; if (chassis_vol > REFEREE_VOLTAGE_VALID_MIN && @@ -362,6 +367,9 @@ static void ControlSwitch() chassis_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE; chassis_cmd_recv.vx = RCChannelToRatio(rc_data[TEMP].rc.rocker_r1) * STOOL_SPEED_REF; +#ifdef REMOTE_FS_I6 + chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF; +#else if (switch_is_up(switch_right)) { chassis_cmd_recv.offset_angle = -STOOL_SPIN_YAW_RATE_REF; @@ -371,6 +379,7 @@ static void ControlSwitch() { chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF; } +#endif } /* Joint motors must remain disabled on this demo chassis. */ diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 930ff2f..43d3df3 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -6,11 +6,24 @@ #include "daemon.h" #include "bsp_log.h" -#define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小 +#ifdef REMOTE_FS_I6 +#define REMOTE_CONTROL_FRAME_SIZE 25u +#define SBUS_HEAD 0x0Fu +#define SBUS_END 0x00u +#define SBUS_FLAG_FRAME_LOST (1u << 2) +#define SBUS_FLAG_FAILSAFE (1u << 3) +#define FSI6_CHANNEL_VALID_MIN 172u +#define FSI6_CHANNEL_VALID_MAX 1811u +#define FSI6_SWITCH_UP_MAX 700u +#define FSI6_SWITCH_DOWN_MIN 1350u +#else +#define REMOTE_CONTROL_FRAME_SIZE 18u +#endif // 遥控器数据 static RC_ctrl_t rc_ctrl[2] = {0}; //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断 static uint8_t rc_init_flag = 0; // 遥控器初始化标志位 +static uint8_t rc_data_valid = 0; // 遥控器拥有的串口实例,因为遥控器是单例,所以这里只有一个,就不封装了 static USARTInstance *rc_usart_instance; @@ -20,19 +33,22 @@ static DaemonInstance *rc_daemon_instance; * @brief 矫正遥控器摇杆的值,超过660或者小于-660的值都认为是无效值,置0 * */ +#ifdef REMOTE_DJI_DT7 static void RectifyRCjoystick() { for (uint8_t i = 0; i < 5; ++i) if (abs(*(&rc_ctrl[TEMP].rc.rocker_l_ + i)) > 660) *(&rc_ctrl[TEMP].rc.rocker_l_ + i) = 0; } +#endif /** * @brief 遥控器数据解析 * * @param sbus_buf 接收buffer */ -static void sbus_to_rc(const uint8_t *sbus_buf) +#ifdef REMOTE_DJI_DT7 +static uint8_t sbus_to_rc(const uint8_t *sbus_buf) { // 摇杆,直接解算时减去偏置 rc_ctrl[TEMP].rc.rocker_r_ = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 0 @@ -85,7 +101,84 @@ static void sbus_to_rc(const uint8_t *sbus_buf) } memcpy(&rc_ctrl[LAST], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断 + return 1; } +#endif + +#ifdef REMOTE_FS_I6 +static uint8_t FSi6EndByteIsValid(uint8_t value) +{ + return value == SBUS_END || value == 0x04u || value == 0x14u || + value == 0x24u || value == 0x34u; +} + +static uint16_t FSi6DecodeChannel(const uint8_t *sbus_buf, uint8_t channel) +{ + uint16_t bit_index = (uint16_t)channel * 11u; + uint8_t byte_index = (uint8_t)(1u + bit_index / 8u); + uint8_t bit_offset = (uint8_t)(bit_index % 8u); + uint32_t packed = (uint32_t)sbus_buf[byte_index] | + ((uint32_t)sbus_buf[byte_index + 1u] << 8u) | + ((uint32_t)sbus_buf[byte_index + 2u] << 16u); + return (uint16_t)((packed >> bit_offset) & 0x07FFu); +} + +static int16_t FSi6NormalizeChannel(uint16_t value) +{ + int32_t normalized = ((int32_t)value - (int32_t)RC_CH_VALUE_OFFSET) * 660 / 784; + if (normalized > 660) + normalized = 660; + else if (normalized < -660) + normalized = -660; + return (int16_t)normalized; +} + +static uint8_t FSi6DecodeSwitch(uint16_t value) +{ + if (value < FSI6_SWITCH_UP_MAX) + return RC_SW_UP; + if (value > FSI6_SWITCH_DOWN_MIN) + return RC_SW_DOWN; + return RC_SW_MID; +} + +static uint8_t sbus_to_rc(const uint8_t *sbus_buf) +{ + if (sbus_buf[0] != SBUS_HEAD || !FSi6EndByteIsValid(sbus_buf[24]) || + (sbus_buf[23] & (SBUS_FLAG_FRAME_LOST | SBUS_FLAG_FAILSAFE)) != 0u) + return 0; + + for (uint8_t i = 0; i < 10u; i++) + { + uint16_t value = FSi6DecodeChannel(sbus_buf, i); + if (value < FSI6_CHANNEL_VALID_MIN || value > FSI6_CHANNEL_VALID_MAX) + return 0; + rc_ctrl[TEMP].rc.ch[i] = value; + } + + /* Keep the app-facing layout compatible with DT7 control semantics. */ + rc_ctrl[TEMP].rc.rocker_l_ = FSi6NormalizeChannel(rc_ctrl[TEMP].rc.ch[0]); + rc_ctrl[TEMP].rc.rocker_l1 = FSi6NormalizeChannel(rc_ctrl[TEMP].rc.ch[1]); + rc_ctrl[TEMP].rc.rocker_r1 = FSi6NormalizeChannel(rc_ctrl[TEMP].rc.ch[2]); + rc_ctrl[TEMP].rc.rocker_r_ = FSi6NormalizeChannel(rc_ctrl[TEMP].rc.ch[3]); + rc_ctrl[TEMP].rc.dial = FSi6NormalizeChannel(rc_ctrl[TEMP].rc.ch[8]); + + rc_ctrl[TEMP].sw_a = FSi6DecodeSwitch(rc_ctrl[TEMP].rc.ch[4]); + rc_ctrl[TEMP].sw_b = FSi6DecodeSwitch(rc_ctrl[TEMP].rc.ch[5]); + rc_ctrl[TEMP].sw_c = FSi6DecodeSwitch(rc_ctrl[TEMP].rc.ch[6]); + rc_ctrl[TEMP].sw_d = FSi6DecodeSwitch(rc_ctrl[TEMP].rc.ch[7]); + + /* SWC selects the auxiliary app mode; SWD is the rightmost safety switch. */ + rc_ctrl[TEMP].rc.switch_left = rc_ctrl[TEMP].sw_c; + rc_ctrl[TEMP].rc.switch_right = rc_ctrl[TEMP].sw_d; + + memset(&rc_ctrl[TEMP].mouse, 0, sizeof(rc_ctrl[TEMP].mouse)); + memset(&rc_ctrl[TEMP].key, 0, sizeof(rc_ctrl[TEMP].key)); + memset(&rc_ctrl[TEMP].key_count, 0, sizeof(rc_ctrl[TEMP].key_count)); + memcpy(&rc_ctrl[LAST], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); + return 1; +} +#endif /** * @brief 对sbus_to_rc的简单封装,用于注册到bsp_usart的回调函数中 @@ -93,8 +186,18 @@ static void sbus_to_rc(const uint8_t *sbus_buf) */ static void RemoteControlRxCallback() { - DaemonReload(rc_daemon_instance); // 先喂狗 - sbus_to_rc(rc_usart_instance->recv_buff); // 进行协议解析 + if (sbus_to_rc(rc_usart_instance->recv_buff)) + { + rc_data_valid = 1; + DaemonReload(rc_daemon_instance); + } +#ifdef REMOTE_FS_I6 + else + { + rc_data_valid = 0; + memset(rc_ctrl, 0, sizeof(rc_ctrl)); + } +#endif } /** @@ -103,6 +206,7 @@ static void RemoteControlRxCallback() */ static void RCLostCallback(void *id) { + rc_data_valid = 0; memset(rc_ctrl, 0, sizeof(rc_ctrl)); // 清空遥控器数据 USARTServiceInit(rc_usart_instance); // 尝试重新启动接收 LOGWARNING("[rc] remote control lost"); @@ -131,6 +235,6 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle) uint8_t RemoteControlIsOnline() { if (rc_init_flag) - return DaemonIsOnline(rc_daemon_instance); + return rc_data_valid && DaemonIsOnline(rc_daemon_instance); return 0; -} \ No newline at end of file +} diff --git a/modules/remote/remote_control.h b/modules/remote/remote_control.h index c12f810..b3ab0bf 100644 --- a/modules/remote/remote_control.h +++ b/modules/remote/remote_control.h @@ -17,6 +17,21 @@ #include "main.h" #include "usart.h" +/* Change this one macro to select the remote control protocol. */ +#define REMOTE_TYPE_DJI_DT7 1 +#define REMOTE_TYPE_FS_I6 2 +#ifndef REMOTE_CONTROL_TYPE +#define REMOTE_CONTROL_TYPE REMOTE_TYPE_FS_I6 // Change to REMOTE_TYPE_DJI_DT7 for DT7. +#endif + +#if REMOTE_CONTROL_TYPE == REMOTE_TYPE_DJI_DT7 +#define REMOTE_DJI_DT7 +#elif REMOTE_CONTROL_TYPE == REMOTE_TYPE_FS_I6 +#define REMOTE_FS_I6 +#else +#error "Unsupported remote control protocol" +#endif + // 用于遥控器数据读取,遥控器数据是一个大小为2的数组 #define LAST 1 #define TEMP 0 @@ -99,6 +114,10 @@ typedef struct uint8_t switch_left; // 左侧开关 uint8_t switch_right; // 右侧开关 + +#ifdef REMOTE_FS_I6 + uint16_t ch[10]; // FSi6原始SBUS通道 +#endif } rc; struct { @@ -111,6 +130,13 @@ typedef struct Key_t key[3]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍 uint8_t key_count[3][16]; + +#ifdef REMOTE_FS_I6 + uint8_t sw_a; + uint8_t sw_b; + uint8_t sw_c; + uint8_t sw_d; +#endif } RC_ctrl_t; /* ------------------------- Internal Data ----------------------------------- */