204 lines
4.8 KiB
C
204 lines
4.8 KiB
C
|
||
/**
|
||
* 东北大学ACTION码盘驱动
|
||
*
|
||
* 本驱动采用ops_9定位系统，负责收发码盘的原始数据，并在任务中解析出相应的位置坐标。
|
||
* 解析后的位置坐标将被交给导航处理模块，生成期望的运动向量。
|
||
*
|
||
* 使用手册：
|
||
* 请参阅相关文档了解如何使用本驱动。
|
||
*
|
||
* 注意：
|
||
* 本驱动仅适用于东北大学ACTION码盘。
|
||
|
||
*删去Action_HandleOffline错误处理函数中对结构体清0的函数
|
||
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "tim.h"
|
||
#include "Action.h"
|
||
#include <string.h>
|
||
|
||
static osThreadId_t thread_alert;
|
||
|
||
|
||
uint8_t rxbuf[RS232_FRAME_LENGTH];
|
||
|
||
static void Ops10msTimerCallback(void *arg){
|
||
(void)arg;
|
||
osThreadFlagsSet(thread_alert,SIGNAL_OPSTIMER_REDY);
|
||
}
|
||
|
||
/* Private function -------------------------------------------------------- */
|
||
static void ACTION_IdleCallback(void) {
|
||
osThreadFlagsSet(thread_alert,SIGNAL_ACTION_RAW_REDY);
|
||
}
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
int8_t ACTIONRECV_Init(Action_POS_t *pos){
|
||
if(pos == NULL) return DEVICE_ERR_NULL;
|
||
|
||
pos->Action_ready =0;//码盘校准标志位初始化
|
||
|
||
if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
|
||
pos->action_timer_Id =
|
||
osTimerNew(Ops10msTimerCallback, osTimerPeriodic,NULL,NULL);
|
||
|
||
osTimerStart(pos->action_timer_Id,10);//此处ticks 理解为1个tick为1ms 此处为每10ms触发一次回调函数
|
||
|
||
BSP_UART_RegisterCallback(BSP_UART_ACTION,BSP_UART_IDLE_LINE_CB,
|
||
ACTION_IdleCallback);
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
int8_t ACTION_StartReceiving() {
|
||
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_ACTION),
|
||
(uint8_t *)rxbuf,
|
||
sizeof(rxbuf)) == HAL_OK)
|
||
return DEVICE_OK;
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
bool_t ACTION_WaitDmaCplt(void) {
|
||
return(osThreadFlagsWait(SIGNAL_ACTION_RAW_REDY, osFlagsWaitAll,400) ==
|
||
SIGNAL_ACTION_RAW_REDY);
|
||
}
|
||
|
||
|
||
/* *
|
||
|
||
|
||
*/
|
||
int8_t ACTION_Parse(Action_POS_t *pos)
|
||
{
|
||
if (pos == NULL) return DEVICE_ERR_NULL;
|
||
|
||
static union
|
||
{
|
||
fp32 pos_data[3];
|
||
char rxbuff[12];
|
||
} pos_rxbuf;
|
||
|
||
|
||
if (rxbuf[0] == 0x0D && rxbuf[1] == 0x0A)
|
||
{
|
||
if (rxbuf[26] == 0x0A && rxbuf[27] == 0x0D)
|
||
{
|
||
|
||
pos_rxbuf.rxbuff[0]=rxbuf[2];
|
||
pos_rxbuf.rxbuff[1]=rxbuf[3];
|
||
pos_rxbuf.rxbuff[2]=rxbuf[4];
|
||
pos_rxbuf.rxbuff[3]=rxbuf[5];
|
||
|
||
|
||
pos_rxbuf.rxbuff[4]=rxbuf[14];
|
||
pos_rxbuf.rxbuff[5]=rxbuf[15];
|
||
pos_rxbuf.rxbuff[6]=rxbuf[16];
|
||
pos_rxbuf.rxbuff[7]=rxbuf[17];
|
||
|
||
|
||
pos_rxbuf.rxbuff[8]=rxbuf[18];
|
||
pos_rxbuf.rxbuff[9]=rxbuf[19];
|
||
pos_rxbuf.rxbuff[10]=rxbuf[20];
|
||
pos_rxbuf.rxbuff[11]=rxbuf[21];
|
||
|
||
|
||
|
||
// 数据解析
|
||
pos->pos_yaw = pos_rxbuf.pos_data[0];
|
||
// 按照安装方向决定正负号
|
||
pos->pos_x = pos_rxbuf.pos_data[1];
|
||
pos->pos_y = pos_rxbuf.pos_data[2];
|
||
|
||
pos ->Action_ready =1;//码盘校准完成
|
||
}
|
||
}
|
||
else
|
||
{
|
||
return -1;
|
||
}
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
//该函数用来计算速度(利用获取的位置来计算相应的速度)
|
||
//此处获取的是真实的位置(mm)后每10ms做一次微分处理
|
||
int8_t ACTION_DataRefresh(Action_POS_t *pos){
|
||
if (pos == NULL) return DEVICE_ERR_NULL;
|
||
|
||
if (osThreadFlagsGet() & SIGNAL_OPSTIMER_REDY){
|
||
osThreadFlagsClear(SIGNAL_OPSTIMER_REDY);
|
||
//计算每毫秒的速度 该函数每10ms调用
|
||
pos->pos_Vx = (pos->pos_x - pos->pos_lastX) / 10;
|
||
pos->pos_Vy = (pos->pos_y - pos->pos_lastY) / 10;
|
||
|
||
pos->pos_lastX = pos->pos_x;
|
||
pos->pos_lastY = pos->pos_y;
|
||
}
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
|
||
|
||
int8_t Action_HandleOffline(Action_POS_t *pos) {
|
||
if (pos == NULL) return DEVICE_ERR_NULL;
|
||
|
||
(void)pos;
|
||
|
||
// memset(pos, 0, sizeof(*pos));
|
||
return 0;
|
||
}
|
||
|
||
/* 将字符串拼接 */
|
||
void Strcat(char str1[], char str2[], uint8_t num)
|
||
{
|
||
int i = 0, j = 0;
|
||
|
||
while (str1[i] != '\0')
|
||
i++;
|
||
|
||
for (j = 0; j < num; j++)
|
||
{
|
||
str1[i++] = str2[j];
|
||
}
|
||
}
|
||
|
||
/* 该部分函数用于码盘的重定位系统 */
|
||
/* ----------------------------- */
|
||
|
||
/* 手动标定，用于校正 */
|
||
void ACT_Calibration(void)
|
||
{
|
||
HAL_UART_Transmit(&huart1, (uint8_t *)"ACTR", 4, 100);
|
||
}
|
||
|
||
/* 清零 */
|
||
/* 将当前位置设置为 (0, 0) */
|
||
void ACT_ZeroClear(void)
|
||
{
|
||
HAL_UART_Transmit(&huart1, (uint8_t *)"ACT0", 4, 100);
|
||
}
|
||
|
||
/* 更新 XY 坐标 */
|
||
/* 将当前位置更新为指定的 X 和 Y */
|
||
void ACT_UpdateXY(float pos_x, float pos_y)
|
||
{
|
||
char update_xy[12] = "ACTD"; // 命令前缀为 "ACTD"
|
||
static union
|
||
{
|
||
float XY[2]; // 两个浮点数表示 X 和 Y 坐标
|
||
char data[8]; // 将浮点数视为字节数据
|
||
} set;
|
||
|
||
set.XY[0] = pos_x;
|
||
set.XY[1] = pos_y;
|
||
|
||
Strcat(update_xy, set.data, 8);
|
||
|
||
HAL_UART_Transmit(&huart1, (uint8_t *)update_xy, sizeof(update_xy), 100);
|
||
}
|
||
|
||
|
||
|