文章目录
- radiolinkTask无线连接任务
- usblinkTxTask usb发送任务
- usblinkRxTask usb接收任务
- commanderTask飞控指令发送任务
- keyTask按键扫描任务
- displayTask显示任务
- configParamTask参数配置任务
- radiolinkDataProcessTask无线通信数据处理任务
- usblinkDataProcessTaskUSB通信数据处理任务
radiolinkTask无线连接任务
xTaskCreate(radiolinkTask, "RADIOLINK", 100, NULL, 6, &radiolinkTaskHandle);
radiolinkTask 主要功能是发送 ATKP 数据包给四轴,并接收四轴返回的应答包。
radiolinkTask 函数在 radiolink.c 中。
void radiolinkTask(void* param)
{
u8 rx_len;
atkp_t rx_p;
while(1)
{
nrf_txPacket((u8*)&tx_p, tx_p.dataLen+2);
xSemaphoreTake(nrfIT, 1000);
nrfEvent_e status = nrf_checkEventandRxPacket((u8*)&rx_p, &rx_len);
if(status == RX_DR)
{
LED_BLUE = 0;
LED_RED = 1;
statusCount = 0;
connectStatus = true;
if(rx_p.dataLen <= ATKP_MAX_DATA_SIZE)
{
xQueueSend(rxQueue, &rx_p, portMAX_DELAY);
}
if(xQueueReceive(txQueue, &tx_p, 0) == pdFALSE)
{
tx_p.msgID = DOWN_RADIO;
tx_p.dataLen = 1;
tx_p.data[0] = D_RADIO_HEARTBEAT;
}
}
else if(status == MAX_RT)
{
LED_BLUE = 1;
LED_RED = 0;
failRxCount++;
if(++statusCount > 10)
{
statusCount = 0;
connectStatus = false;
}
}
if(connectStatus==true && xTaskGetTickCount()>=failRxcountTime+1000)
{
failRxcountTime = xTaskGetTickCount();
failReceiveNum = failRxCount;
failRxCount = 0;
}
}
}
① 定义8位数据rx_len
、通讯数据结构体rx_p
;
② nrf_txPacket()
发送数据包(PTX模式);
③ nrf_checkEventandRxPacket()
查询事件并接收数据包;
④ 若发送成功,则蓝灯亮,执行相应操作;若发送失败,则红灯亮,如果连续10次无应答则通讯失败;
⑤ 1000ms统计一次收发失败次数。
usblinkTxTask usb发送任务
xTaskCreate(usblinkTxTask, "USBLINK_TX", 100, NULL, 5, NULL);
usblinkTxTask 主要功能是给 ATKP 数据包加上帧头和校验并发送给上位机。
usblinkTxTask 函数在 usblink.c 中。
void usblinkTxTask(void* param)
{
atkp_t p;
u8 sendBuffer[64];
u8 cksum;
u8 dataLen;
while(bDeviceState != CONFIGURED)
{
vTaskDelay(1000);
}
while(1)
{
xQueueReceive(txQueue, &p, portMAX_DELAY);
if(p.msgID != UP_RADIO)
{
if(p.msgID == UP_PRINTF)
{
memcpy(&sendBuffer, p.data, p.dataLen);
dataLen = p.dataLen;
}
else
{
sendBuffer[0] = UP_BYTE1;
sendBuffer[1] = UP_BYTE2;
sendBuffer[2] = p.msgID;
sendBuffer[3] = p.dataLen;
memcpy(&sendBuffer[4], p.data, p.dataLen);
cksum = 0;
for (int i=0; i<p.dataLen+4; i++)
{
cksum += sendBuffer[i];
}
dataLen = p.dataLen+5;
sendBuffer[dataLen - 1] = cksum;
}
usbsendData(sendBuffer, dataLen);
}
}
}
① 定义相关数据与结构体,然后等待usb配置成功;
② 当NRF51822的数据包不上传时,若p.msgID == UP_PRINTF
,则打印数据包去掉帧头;否则执行相应操作。
usblinkRxTask usb接收任务
xTaskCreate(usblinkRxTask, "USBLINK_RX", 100, NULL, 5, NULL);
usblinkRxTask 主要功能是接收上位机发下来的串口数据,按照 ATKP 格式打包。
usblinkRxTask 函数在 usblink.c 中。
void usblinkRxTask(void *param)
{
u8 c;
u8 dataIndex = 0;
u8 cksum = 0;
rxState = waitForStartByte1;
while(1)
{
if (usbGetDataWithTimout(&c))
{
switch(rxState)
{
case waitForStartByte1:
rxState = (c == DOWN_BYTE1) ? waitForStartByte2 : waitForStartByte1;
cksum = c;
break;
case waitForStartByte2:
rxState = (c == DOWN_BYTE2) ? waitForMsgID : waitForStartByte1;
cksum += c;
break;
case waitForMsgID:
rxPacket.msgID = c;
rxState = waitForDataLength;
cksum += c;
break;
case waitForDataLength:
if (c <= ATKP_MAX_DATA_SIZE)
{
rxPacket.dataLen = c;
dataIndex = 0;
rxState = (c > 0) ? waitForData : waitForChksum1;
cksum += c;
} else
{
rxState = waitForStartByte1;
}
break;
case waitForData:
rxPacket.data[dataIndex] = c;
dataIndex++;
cksum += c;
if (dataIndex == rxPacket.dataLen)
{
rxState = waitForChksum1;
}
break;
case waitForChksum1:
if (cksum == c)
{
xQueueSend(rxQueue, &rxPacket, 0);
}
else
{
rxState = waitForStartByte1;
}
rxState = waitForStartByte1;
break;
default:
break;
}
}
else
{
rxState = waitForStartByte1;
}
}
}
① 相关数据与结构体的定义与赋值;
② 若满足usbGetDataWithTimout(&c)
的条件,根据rxState
的值执行相应操作,其中rxState = (c > 0) ? waitForData : waitForChksum1
则为c=0,数据长度为0,校验1,cksum == c
为所有校验正确。
commanderTask飞控指令发送任务
xTaskCreate(commanderTask, "COMMANDER", 100, NULL, 4, NULL);
commanderTask 主要功能是将采集摇杆电位器的 AD 值转换为姿态控制命令,并以 10ms的周期通过 radiolink 链路发送给四轴。
commanderTask 函数在 remoter_ctrl.c 中。
void commanderTask(void* param)
{
float max_thrust = LOW_SPEED_THRUST;
float max_pitch = LOW_SPEED_PITCH;
float max_roll = LOW_SPEED_ROLL;
joystickFlyf_t percent;
while(1)
{
vTaskDelay(10);
switch(configParam.flight.speed)
{
case LOW_SPEED:
max_thrust = LOW_SPEED_THRUST;
max_pitch = LOW_SPEED_PITCH;
max_roll = LOW_SPEED_ROLL;
break;
case MID_SPEED:
max_thrust = MID_SPEED_THRUST;
max_pitch = MID_SPEED_PITCH;
max_roll = MID_SPEED_ROLL;
break;
case HIGH_SPEED:
max_thrust = HIGH_SPEED_THRUST;
max_pitch = HIGH_SPEED_PITCH;
max_roll = HIGH_SPEED_ROLL;
break;
}
ADCtoFlyDataPercent(&percent);
if(configParam.flight.ctrl == ALTHOLD_MODE || configParam.flight.ctrl == THREEHOLD_MODE)
{
flydata.thrust = percent.thrust * ALT_THRUST;
flydata.thrust += ALT_THRUST;
flydata.thrust = limit(flydata.thrust, 0, 100);
}
else
{
flydata.thrust = percent.thrust * (max_thrust - MIN_THRUST);
flydata.thrust += MIN_THRUST;
flydata.thrust = limit(flydata.thrust, MIN_THRUST, max_thrust);
}
flydata.roll = percent.roll * max_roll;
flydata.roll = limit(flydata.roll, -max_roll, max_roll);
flydata.pitch = percent.pitch * max_pitch;
flydata.pitch = limit(flydata.pitch, -max_pitch, max_pitch);
flydata.yaw = percent.yaw * MAX_YAW;
flydata.yaw = limit(flydata.yaw, -MAX_YAW, MAX_YAW);
if(getRCLock()==false && radioinkConnectStatus()==true && getIsMFCanFly()==true)
{
remoterData_t send;
switch(configParam.flight.mode)
{
case HEAD_LESS:
send.flightMode = 1;
break;
case X_MODE:
send.flightMode = 0;
break;
}
switch(configParam.flight.ctrl)
{
case ALTHOLD_MODE:
send.ctrlMode = 1;
break;
case MANUAL_MODE:
send.ctrlMode = 0;
break;
case THREEHOLD_MODE:
send.ctrlMode = 3;
break;
}
if(flydata.thrust<=MIN_THRUST && send.ctrlMode==0)
{
send.thrust = 0;
}
else
{
send.thrust = flydata.thrust;
}
if(getTrimFlag() == true)
{
send.pitch = 0;
send.roll = 0;
}
else
{
send.pitch = flydata.pitch ;
send.roll = flydata.roll;
}
send.yaw = flydata.yaw;
send.trimPitch = configParam.trim.pitch;
send.trimRoll = configParam.trim.roll;
sendRmotorData((u8*)&send, sizeof(send));
}
if(radioinkConnectStatus()==true)
{
atkp_t p;
joystickFlyui16_t rcdata;
rcdata.thrust = flydata.thrust*10 + 1000;
rcdata.pitch = percent.pitch*500 + 1500;
rcdata.roll = percent.roll*500 + 1500;
rcdata.yaw = percent.yaw*500 + 1500;
p.msgID = DOWN_RCDATA;
p.dataLen = sizeof(rcdata);
memcpy(p.data, &rcdata, p.dataLen);
radiolinkSendPacket(&p);
}
}
}
joystickFlyf_t getFlyControlData(void)
{
return flydata;
}
① 定义油门值、俯仰角与滚转角的最大值,定义飞控数据结构体percent
,其中
typedef struct
{
float roll;
float pitch;
float yaw;
float thrust;
}joystickFlyf_t;
② 做简短延时后,根据配置参数中的速度speed
来选择油门值、俯仰角与滚转角的值;
③ ADCtoFlyDataPercent(&percent)
将ADC值转换成飞控数据百分比;
④ 根据飞行模式是定高模式/定点模式还是手动模式来配置飞控数据的油门值;
⑤ 配置飞控数据的滚转角、俯仰角与偏航角;
⑥ 发送飞控数据:定义遥控数据结构,根据相应参数的值配置要发送的遥控数据结构的值,通过sendRmotorData()
发送遥控控制数据;
⑦ 发送遥感数据至匿名上位机。
keyTask按键扫描任务
xTaskCreate(keyTask, "BUTTON_SCAN", 100, NULL, 3, NULL);
keyTask 主要功能是扫描按键,根据按键按下的时间来区分长按和短按。
keyTask 函数在 keyTask.c 中。
void keyTask(void* param)
{
while(1)
{
vTaskDelay(20);
if(keyL_pressed==false && READ_KEY_L()==PRESSED)
{
keyL_pressed = true;
pressedTime = xTaskGetTickCount();
}
if(keyR_pressed==false && READ_KEY_R()==PRESSED)
{
keyR_pressed = true;
pressedTime = xTaskGetTickCount();
}
if(keyJ1_pressed==false && READ_KEY_J1()==PRESSED)
{
keyJ1_pressed = true;
pressedTime = xTaskGetTickCount();
}
if(keyJ2_pressed==false && READ_KEY_J2()==PRESSED)
{
keyJ2_pressed = true;
pressedTime = xTaskGetTickCount();
}
if(keyL_pressed==true)
{
if(READ_KEY_L()==RELEASED)
keyL_pressed = false;
if((xTaskGetTickCount() - pressedTime) > LONG_PRESS_COUNT)
keyState = KEY_L_LONG_PRESS;
else if(READ_KEY_L()==RELEASED)
keyState = KEY_L_SHORT_PRESS;
}
if(keyR_pressed==true)
{
if(READ_KEY_R()==RELEASED)
keyR_pressed = false;
if((xTaskGetTickCount() - pressedTime) > LONG_PRESS_COUNT)
keyState = KEY_R_LONG_PRESS;
else if(READ_KEY_R()==RELEASED)
keyState = KEY_R_SHORT_PRESS;
}
if(keyJ1_pressed==true)
{
if(READ_KEY_J1()==RELEASED)
keyJ1_pressed = false;
if((xTaskGetTickCount() - pressedTime) > LONG_PRESS_COUNT)
keyState = KEY_J1_LONG_PRESS;
else if(READ_KEY_J1()==RELEASED)
keyState = KEY_J1_SHORT_PRESS;
}
if(keyJ2_pressed==true)
{
if(READ_KEY_J2()==RELEASED)
keyJ2_pressed = false;
if((xTaskGetTickCount() - pressedTime) > LONG_PRESS_COUNT)
keyState = KEY_J2_LONG_PRESS;
else if(READ_KEY_J2()==RELEASED)
keyState = KEY_J2_SHORT_PRESS;
}
}
}
displayTask显示任务
xTaskCreate(displayTask, "DISPLAY", 200, NULL, 1, NULL);
displayTask 主要功能是显示界面,50ms 刷新一次界面。
displayTask 函数在 display.c 中。
void displayTask(void* param)
{
while(1)
{
vTaskDelay(50);
switch(show_ui)
{
case MAIN_UI:
main_ui();
break;
case TRIM_UI:
trim_ui();
break;
case MENU_UI:
Menu_Run();
break;
case DEBUG_UI:
debug_ui();
break;
case JOYSTICK_CALIB_UI:
joystickCalib_ui();
break;
case MATCH_UI:
match_ui();
break;
case RESET_UI:
reset_ui();
break;
default:break;
}
GUI_Refresh();
}
}
configParamTask参数配置任务
xTaskCreate(configParamTask, "CONFIG_TASK", 100, NULL, 1, NULL);
configParamTask 主要功能是保存参数。任务中 1000ms 判断一次配置参数有无改变,当有参数改变后 6S 内不再改变则将新参数写入 Flash。这样做的目的是避免在微调四轴时频繁写 Flash,Flash 擦写次数是有限的。
configParamTask 函数在 config_param.c 中。
void configParamTask(void* param)
{
u8 cksum = 0;
static u8 count = 0;
while(1)
{
vTaskDelay(1000);
cksum = calculate_cksum(&configParam);
if(configParam.cksum != cksum)
{
configParam.cksum = cksum;
count = 1;
}
if(count)
{
count ++;
}
if(count > 6)
{
count = 0;
STMFLASH_Write(CONFIG_PARAM_ADDR,(u16*)&configParam,sizeof(configParam)/2);
}
}
}
radiolinkDataProcessTask无线通信数据处理任务
xTaskCreate(radiolinkDataProcessTask, "DATA_PROCESS", 100, NULL, 6, NULL);
radiolinkDataProcessTask 主要功能是处理四轴返回的应答包数据,处理完之后再通过usblink 链路转发给上位机。
radiolinkDataProcessTask 函数在 atkp.c 中。
void radiolinkDataProcessTask(void *param)
{
atkp_t p;
while(1)
{
radiolinkReceivePacketBlocking(&p);
atkpAnalyze(&p);
usblinkSendPacket(&p);
vTaskDelay(1);
}
}
usblinkDataProcessTaskUSB通信数据处理任务
xTaskCreate(usblinkDataProcessTask, "DATA_PROCESS", 100, NULL, 6, NULL);
usblinkDataProcessTask 主要功能是处理上位机发下来的 ATKP 数据包,处理完之后通过radiolink 链路转发给四轴。上位机发下来的 ATKP 数据包由 usblinkRxTask 打包。
usblinkDataProcessTask 函数在 atkp.c 中。
void usblinkDataProcessTask(void *param)
{
atkp_t p;
while(1)
{
usblinkReceivePacketBlocking(&p);
atkpAnalyze(&p);
radiolinkSendPacket(&p);
}
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)