TuxMonkey/bf_original_balance_chassis
1
1
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-10-23 23:42:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

fsi6控

Browse Source
This commit is contained in:
TuxMonkey
2026-08-17 23:49:29 +08:00
parent 14a3ab4cd0
commit fc6ac7a0b4
3 changed files with 146 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
application/chassis/balance.c
View File

@@ -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. */

114
modules/remote/remote_control.c
View File

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

26
modules/remote/remote_control.h
View File

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