robot/controller/msp_MK32.c

/******************************************************************************

                  版权所有 (C), 2018-2099, Radkil

 ******************************************************************************
  文 件 名   : msp_MK32.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年9月10日
  最近修改   :
  功能描述   : MK32驱动文件
  
  修改历史   :
  1.日    期   : 2026年9月10日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#include "BHBF.h"
#include "msg_center.h"
#include "msp_MK32.pb.h"
#include "rd_time.h"
#include "msp_MK32.h"

/*----------------------------------------------*
 * 外部变量说明                                 *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 外部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/
static SP_MSP_MK32_Button RB_MK32;
/*----------------------------------------------*
 * 常量定义                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/

int check_MK32(char *_pBuffer, uint32_t _iSize) 
{
  if (_iSize < 25)
    return 0; // 数据不足25字节，不读[24]不误判，等下一包
  if (_pBuffer[0] != 0x0f)
    return -1;
  if (_pBuffer[24] != 0x00)
    return -1;
  return 25;
}

void decode_MK32(const char *buf, uint32_t _iSize) 
{
  static uint32_t uiLastSendTick = 0;

  int32_t CH[16];
  int Start_byte = 0;

  CH[0] = ((buf[Start_byte + 1] | buf[Start_byte + 2] << 8) & 0x07FF);
  CH[1] = ((buf[Start_byte + 2] >> 3 | buf[Start_byte + 3] << 5) & 0x07FF);
  CH[2] = ((buf[Start_byte + 3] >> 6 | buf[Start_byte + 4] << 2 |
            buf[Start_byte + 5] << 10) &
           0x07FF);
  CH[3] = ((buf[Start_byte + 5] >> 1 | buf[Start_byte + 6] << 7) & 0x07FF);
  CH[4] = ((buf[Start_byte + 6] >> 4 | buf[Start_byte + 7] << 4) & 0x07FF);
  CH[5] = ((buf[Start_byte + 7] >> 7 | buf[Start_byte + 8] << 1 |
            buf[Start_byte + 9] << 9) &
           0x07FF);
  CH[6] = ((buf[Start_byte + 9] >> 2 | buf[Start_byte + 10] << 6) & 0x07FF);
  CH[7] = ((buf[Start_byte + 10] >> 5 | buf[Start_byte + 11] << 3) & 0x07FF);
  CH[8] = ((buf[Start_byte + 12] | buf[Start_byte + 13] << 8) & 0x07FF);
  CH[9] = ((buf[Start_byte + 13] >> 3 | buf[Start_byte + 14] << 5) & 0x07FF);
  CH[10] = ((buf[Start_byte + 14] >> 6 | buf[Start_byte + 15] << 2 |
             buf[Start_byte + 16] << 10) &
            0x07FF);
  CH[11] = ((buf[Start_byte + 16] >> 1 | buf[Start_byte + 17] << 7) & 0x07FF);
  CH[12] = ((buf[Start_byte + 17] >> 4 | buf[Start_byte + 18] << 4) & 0x07FF);
  CH[13] = ((buf[Start_byte + 18] >> 7 | buf[Start_byte + 19] << 1 |
             buf[Start_byte + 20] << 9) &
            0x07FF);
  CH[14] = ((buf[Start_byte + 20] >> 2 | buf[Start_byte + 21] << 6) & 0x07FF);
  CH[15] = ((buf[Start_byte + 21] >> 5 | buf[Start_byte + 22] << 3) & 0x07FF);

  // 按键数值转换：1050为中间值，272-1712
  for (int i = 0; i < 16; i++) 
  {
    CH[i] = (int32_t)((CH[i] - 992) * 1.388889);
  }

  RD_MEMCPY(&(RB_MK32.CH0_RY_H), CH, sizeof(CH));

  if (buf[23] == 0) 
  {
    RB_MK32.IsOnline = 1;
  } 
  else 
  {
    RB_MK32.IsOnline = 0;
  }
  RB_MK32.RxIndex++;

  uint32_t uiNowTick = Rd_GetTime();
  if ((int32_t)(uiNowTick - uiLastSendTick) >= 10) 
  {
    uiLastSendTick = uiNowTick;
    MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_MK32, (void *)&RB_MK32,
                     sizeof(RB_MK32));
    MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_MK32_SBUS, NULL, 0);
  }
}

void Read_MK32(void *argument) 
{
  char pcBuffer[52] = {0};

  while (1) 
  {
    rd_ComRead(g_ptSbus, pcBuffer, 25);
  }
}

void controller_init(void)
{
  TUartUserData *ptSbus = UART_userdata_init(5, -1, 512);
  g_ptSbus = rd_ComCreate(check_MK32, decode_MK32, E28_SBUS_Send, ptSbus->m_buf_size, ptSbus);
  UART_IT_init(g_ptSbus);

  const osThreadAttr_t MK32_Task_attributes = {
      .name = "Read_MK32",
      .stack_size = THREAD_DEFAULT_STACK,
      .priority = (osPriority_t) osPriorityRealtime1,
  };
  (void)osThreadNew(Read_MK32, NULL, &MK32_Task_attributes);
}

extern const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX];

#define SAFE_DISPATCH(key, mode, submode)       \
    do {                                        \
        if ((key) < KEY_MAX &&                  \
            (mode) < MODE_MAX &&                \
            (submode) < SUB_MODE_MAX) {          \
        	ActionFunc _fn = g_ActionTable[(key)][(mode)][(submode)]; \
            if (_fn) _fn();                     \
        }                                       \
    } while (0)

WEAK void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode)
{
	if (_iMode <= MODE_NULL || _iMode >= MODE_MAX) return;
	// 急停
	if (_pstMK32->CH8_SE == -1000 && _pstMK32->CH9_SF == -1000)
	{
		SAFE_DISPATCH(E_STOP, _iMode, _iSubMode);
		return;
	}

	// 根据模式判断换道（仅在竖直向左或向右生效）
	if (_pstMK32->CH4_SA == 1000)
	{
		SAFE_DISPATCH(KEY_SA_UP, _iMode, _iSubMode);
		return;
	}
	else if (_pstMK32->CH4_SA == -1000)
	{
		SAFE_DISPATCH(KEY_SA_DOWN, _iMode, _iSubMode);
		return;
	}
	else // 重置前进计时器以便下一次换道重新计数
	{
		SAFE_DISPATCH(KEY_SA_DEFAULT, _iMode, _iSubMode);
	}

	// 自动巡航
	if (_pstMK32->CH5_SB == -1000)
	{
		SAFE_DISPATCH(KEY_SB_UP, _iMode, _iSubMode);
		return;
	}
	else if(_pstMK32->CH5_SB == 1000)
	{
		SAFE_DISPATCH(KEY_SB_DOWN, _iMode, _iSubMode);
		return;
	}


	// 开关喷枪
	if (_pstMK32->CH6_SC == -1000)
	{
		SAFE_DISPATCH(KEY_SC_UP, _iMode, _iSubMode);
	}
	else if (_pstMK32->CH6_SC == 1000)
	{
		SAFE_DISPATCH(KEY_SC_DOWN, _iMode, _iSubMode);
	}
	else
	{
		SAFE_DISPATCH(KEY_SC_DEFAULT, _iMode, _iSubMode);
	}

	// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
	if (abs(_pstMK32->CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(_pstMK32->CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance)
	{
		SAFE_DISPATCH(JOY_DEFAULT, _iMode, _iSubMode);
		return;
	}
	// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效

	// 摇杆角度死区判断
	int angle = atan2(_pstMK32->CH2_LY_V, _pstMK32->CH3_LY_H) * 180 / M_PI;

	if (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance)
	{
		SAFE_DISPATCH(JOY_LEFT_UP, _iMode, _iSubMode);
	}
	else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
	{
		SAFE_DISPATCH(JOY_LEFT_DWON, _iMode, _iSubMode);
	}
	else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
	{
		SAFE_DISPATCH(JOY_LEFT_RIGHT, _iMode, _iSubMode);
	}
	else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance || abs(angle - (-180)) <= g_stCV.Joy_Sticker_Angle_Allowance)
	{
		SAFE_DISPATCH(JOY_LEFT_LEFT, _iMode, _iSubMode);
	}
	else
	{
		MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
	}
}