80 lines
2.0 KiB
C
80 lines
2.0 KiB
C
#include "user_task.h"
|
|
#include "nuc.h"
|
|
|
|
#ifdef DEBUG
|
|
|
|
NUC_t nuc_raw;
|
|
CMD_NUC_t cmd_fromnuc;
|
|
AHRS_Quaternion_t quat;
|
|
AHRS_Accl_t accl;
|
|
AHRS_Gyro_t gyro;
|
|
|
|
NUC_send_t NUC_send;
|
|
#else
|
|
static NUC_t nuc_raw;
|
|
static CMD_NUC_t cmd_fromnuc;
|
|
static AHRS_Quaternion_t quat;
|
|
static AHRS_Accl_t accl;
|
|
static AHRS_Gyro_t gyro;
|
|
NUC_send_t NUC_send;
|
|
|
|
#endif
|
|
fp32 send[4]={11.0f,45.0,1.f,4.0f};
|
|
int a=0;
|
|
void Task_nuc(void *argument){
|
|
(void)argument; /**/
|
|
|
|
osDelay(TASK_INIT_DELAY_NUC);
|
|
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC;
|
|
|
|
NUC_Init(&nuc_raw);
|
|
|
|
uint32_t tick = osKernelGetTickCount();
|
|
uint32_t last_online_tick = tick;
|
|
while (1)
|
|
{
|
|
#ifdef DEBUG
|
|
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
|
|
task_runtime .freq.nuc = TASK_FREQ_NUC;
|
|
task_runtime.last_up_time.nuc= tick;
|
|
|
|
#endif
|
|
a++;
|
|
NUC_StartReceiving();
|
|
if (NUC_WaitDmaCplt()){
|
|
NUC_RawParse(&cmd_fromnuc, &nuc_raw);
|
|
last_online_tick = tick;
|
|
}
|
|
else{
|
|
if (tick - last_online_tick > 300) NUC_HandleOffline(&cmd_fromnuc, &nuc_raw);
|
|
}
|
|
|
|
if (nuc_raw.unc_online) {
|
|
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
|
|
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
|
|
}
|
|
|
|
osMessageQueueGet(task_runtime.msgq.nuc.quat, &(quat), NULL, 0);
|
|
osMessageQueueGet(task_runtime.msgq.nuc.accl, &(accl), NULL, 0);
|
|
osMessageQueueGet(task_runtime.msgq.nuc.gyro, &(gyro), NULL, 0);
|
|
|
|
// osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);
|
|
|
|
bool cmd_update = (osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send,
|
|
&(NUC_send), NULL, 0) == osOK);
|
|
|
|
NUC_PackIMU(&nuc_raw, &quat, &accl, &gyro);
|
|
if (cmd_update) {
|
|
NUC_PackCMD(&nuc_raw,&NUC_send);
|
|
}
|
|
|
|
|
|
NUC_StartSend(&nuc_raw, cmd_update);
|
|
|
|
tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
|
|
osDelayUntil(tick);
|
|
}
|
|
|
|
}
|
|
|