279 lines
9.0 KiB
C
279 lines
9.0 KiB
C
|
/**
|
|||
|
|
****************************(C) COPYRIGHT 2019 DJI****************************
|
|||
|
|
* @file detect_task.c/h
|
|||
|
|
* @brief detect error task, judged by receiving data time. provide detect
|
|||
|
|
hook function, error exist function.
|
|||
|
|
* 检测错误任务， 通过接收数据时间来判断.提供 检测钩子函数,错误存在函数.
|
|||
|
|
* @note
|
|||
|
|
* @history
|
|||
|
|
* Version Date Author Modification
|
|||
|
|
* V1.0.0 Dec-26-2018 RM 1. done
|
|||
|
|
* V1.1.0 Nov-11-2019 RM 1. add oled, gyro accel and mag sensors
|
|||
|
|
*
|
|||
|
|
@verbatim
|
|||
|
|
==============================================================================
|
|||
|
|
add a sensor
|
|||
|
|
1. in detect_task.h, add the sensor name at the end of errorList,like
|
|||
|
|
enum errorList
|
|||
|
|
{
|
|||
|
|
...
|
|||
|
|
XXX_TOE, //new sensor
|
|||
|
|
ERROR_LIST_LENGHT,
|
|||
|
|
};
|
|||
|
|
2.in detect_init function, add the offlineTime, onlinetime, priority params,like
|
|||
|
|
uint16_t set_item[ERROR_LIST_LENGHT][3] =
|
|||
|
|
{
|
|||
|
|
...
|
|||
|
|
{n,n,n}, //XX_TOE
|
|||
|
|
};
|
|||
|
|
3. if XXX_TOE has data_is_error_fun ,solve_lost_fun,solve_data_error_fun function,
|
|||
|
|
please assign to function pointer.
|
|||
|
|
4. when XXX_TOE sensor data come, add the function detect_hook(XXX_TOE) function.
|
|||
|
|
如果要添加一个新设备
|
|||
|
|
1.第一步在detect_task.h，添加设备名字在errorList的最后，像
|
|||
|
|
enum errorList
|
|||
|
|
{
|
|||
|
|
...
|
|||
|
|
XXX_TOE, //新设备
|
|||
|
|
ERROR_LIST_LENGHT,
|
|||
|
|
};
|
|||
|
|
2.在detect_init函数,添加offlineTime, onlinetime, priority参数
|
|||
|
|
uint16_t set_item[ERROR_LIST_LENGHT][3] =
|
|||
|
|
{
|
|||
|
|
...
|
|||
|
|
{n,n,n}, //XX_TOE
|
|||
|
|
};
|
|||
|
|
3.如果有data_is_error_fun ,solve_lost_fun,solve_data_error_fun函数，赋值到函数指针
|
|||
|
|
4.在XXX_TOE设备数据来的时候, 添加函数detect_hook(XXX_TOE).
|
|||
|
|
==============================================================================
|
|||
|
|
@endverbatim
|
|||
|
|
****************************(C) COPYRIGHT 2019 DJI****************************
|
|||
|
|
*/
|
|||
|
|
#include "error_detect.h"
|
|||
|
|
#include "user_task.h"
|
|||
|
|
#include "r12ds.h"
|
|||
|
|
#include "nuc.h"
|
|||
|
|
/**
|
|||
|
|
* @brief init error_list, assign offline_time, online_time, priority.
|
|||
|
|
* @param[in] time: system time
|
|||
|
|
* @retval none
|
|||
|
|
*/
|
|||
|
|
/**
|
|||
|
|
* @brief 初始化error_list,赋值 offline_time, online_time, priority
|
|||
|
|
* @param[in] time:系统时间
|
|||
|
|
* @retval none
|
|||
|
|
*/
|
|||
|
|
static void detect_init(uint32_t time);
|
|||
|
|
|
|||
|
|
|
|||
|
|
error_t error_list[ERROR_LIST_LENGHT + 1];
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 检测任务
|
|||
|
|
* @param[in] pvParameters: NULL
|
|||
|
|
* @retval none
|
|||
|
|
*/
|
|||
|
|
uint8_t online_dev;
|
|||
|
|
void Task_error_detect(void *argument){
|
|||
|
|
(void)argument; /* 未使用argument，消除警告 */
|
|||
|
|
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_ERROR_DTC;
|
|||
|
|
static uint32_t system_time;
|
|||
|
|
system_time = xTaskGetTickCount();
|
|||
|
|
//init,初始化
|
|||
|
|
detect_init(system_time);
|
|||
|
|
online_dev =0;
|
|||
|
|
//wait a time.空闲一段时间
|
|||
|
|
vTaskDelay(DETECT_TASK_INIT_TIME);
|
|||
|
|
|
|||
|
|
uint32_t tick = osKernelGetTickCount(); /*控制任务运行频率的计时 */
|
|||
|
|
while (1)
|
|||
|
|
{
|
|||
|
|
static uint8_t error_num_display = 0;
|
|||
|
|
system_time = xTaskGetTickCount();
|
|||
|
|
|
|||
|
|
error_num_display = ERROR_LIST_LENGHT;
|
|||
|
|
error_list[ERROR_LIST_LENGHT].is_lost = 0;
|
|||
|
|
error_list[ERROR_LIST_LENGHT].error_exist = 0;
|
|||
|
|
|
|||
|
|
//若所有设备在线，会不断翻转绿灯 存在设备离线则会红灯灯翻转
|
|||
|
|
|
|||
|
|
if(online_dev == ERROR_LIST_LENGHT)
|
|||
|
|
{
|
|||
|
|
HAL_GPIO_TogglePin(GPIOH,LED_G_Pin);
|
|||
|
|
}
|
|||
|
|
else HAL_GPIO_TogglePin(GPIOH,LED_R_Pin);
|
|||
|
|
|
|||
|
|
for (int i = 0; i < ERROR_LIST_LENGHT; i++)
|
|||
|
|
{
|
|||
|
|
if(i == 0) online_dev=0; //每次重新开始循环检测时需要重置在线设备
|
|||
|
|
//disable, continue
|
|||
|
|
//未使能，跳过
|
|||
|
|
if (error_list[i].enable == 0)
|
|||
|
|
{
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
//judge offline.判断掉线
|
|||
|
|
if (system_time - error_list[i].new_time > error_list[i].set_offline_time)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
if (error_list[i].error_exist == 0)
|
|||
|
|
{
|
|||
|
|
//record error and time
|
|||
|
|
//记录错误以及掉线时间
|
|||
|
|
error_list[i].is_lost = 1;
|
|||
|
|
error_list[i].error_exist = 1;
|
|||
|
|
error_list[i].lost_time = system_time;
|
|||
|
|
}
|
|||
|
|
//judge the priority,save the highest priority ,
|
|||
|
|
//判断错误优先级， 保存优先级最高的错误码
|
|||
|
|
if (error_list[i].priority > error_list[error_num_display].priority)
|
|||
|
|
{
|
|||
|
|
error_num_display = i;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
error_list[ERROR_LIST_LENGHT].is_lost = 1;
|
|||
|
|
error_list[ERROR_LIST_LENGHT].error_exist = 1;
|
|||
|
|
//if solve_lost_fun != NULL, run it
|
|||
|
|
//如果提供解决函数，运行解决函数
|
|||
|
|
if (error_list[i].solve_lost_fun != NULL)
|
|||
|
|
{
|
|||
|
|
error_list[i].solve_lost_fun();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if (system_time - error_list[i].work_time < error_list[i].set_online_time)
|
|||
|
|
{
|
|||
|
|
//just online, maybe unstable, only record
|
|||
|
|
//刚刚上线，可能存在数据不稳定，只记录不丢失，
|
|||
|
|
error_list[i].is_lost = 0;
|
|||
|
|
error_list[i].error_exist = 1;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
online_dev++;
|
|||
|
|
error_list[i].is_lost = 0;
|
|||
|
|
//判断是否存在数据错误
|
|||
|
|
//judge if exist data error
|
|||
|
|
if (error_list[i].data_is_error != NULL)
|
|||
|
|
{
|
|||
|
|
error_list[i].error_exist = 1;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
error_list[i].error_exist = 0;
|
|||
|
|
}
|
|||
|
|
//calc frequency
|
|||
|
|
//计算频率
|
|||
|
|
if (error_list[i].new_time > error_list[i].last_time)
|
|||
|
|
{
|
|||
|
|
error_list[i].frequency = configTICK_RATE_HZ / (fp32)(error_list[i].new_time - error_list[i].last_time);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
tick += delay_tick; /*计算下一个唤醒时刻*/
|
|||
|
|
osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 获取设备对应的错误状态
|
|||
|
|
* @param[in] toe:设备目录
|
|||
|
|
* @retval true(错误) 或者false(没错误)
|
|||
|
|
*/
|
|||
|
|
bool_t toe_is_error(uint8_t toe)
|
|||
|
|
{
|
|||
|
|
return (error_list[toe].error_exist == 1);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 记录时间
|
|||
|
|
* @param[in] toe:设备目录
|
|||
|
|
* @retval none
|
|||
|
|
*/
|
|||
|
|
void detect_hook(uint8_t toe)
|
|||
|
|
{
|
|||
|
|
error_list[toe].last_time = error_list[toe].new_time;
|
|||
|
|
error_list[toe].new_time = xTaskGetTickCount();
|
|||
|
|
|
|||
|
|
if (error_list[toe].is_lost)
|
|||
|
|
{
|
|||
|
|
error_list[toe].is_lost = 0;
|
|||
|
|
error_list[toe].work_time = error_list[toe].new_time;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (error_list[toe].data_is_error_fun != NULL)
|
|||
|
|
{
|
|||
|
|
if (error_list[toe].data_is_error_fun())
|
|||
|
|
{
|
|||
|
|
error_list[toe].error_exist = 1;
|
|||
|
|
error_list[toe].data_is_error = 1;
|
|||
|
|
|
|||
|
|
if (error_list[toe].solve_data_error_fun != NULL)
|
|||
|
|
{
|
|||
|
|
error_list[toe].solve_data_error_fun();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
error_list[toe].data_is_error = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
error_list[toe].data_is_error = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 得到错误列表
|
|||
|
|
* @param[in] none
|
|||
|
|
* @retval error_list的指针
|
|||
|
|
*/
|
|||
|
|
const error_t *get_error_list_point(void)
|
|||
|
|
{
|
|||
|
|
return error_list;
|
|||
|
|
}
|
|||
|
|
static void detect_init(uint32_t time)
|
|||
|
|
{
|
|||
|
|
//设置离线时间，上线稳定工作时间，优先级 offlineTime onlinetime priority
|
|||
|
|
uint16_t set_item[ERROR_LIST_LENGHT][3] =
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
// {150, 150, 15}, //NUC
|
|||
|
|
|
|||
|
|
// {30, 30, 10}, //motor1
|
|||
|
|
// {30, 30, 10}, //motor2
|
|||
|
|
// {30, 30, 10}, //motor3
|
|||
|
|
// {30, 30, 10}, //motor4
|
|||
|
|
//pitch
|
|||
|
|
// {30, 30, 10}, //motor5
|
|||
|
|
|
|||
|
|
{15, 5, 7}, //board gyro
|
|||
|
|
{15, 5, 7}, //board accel
|
|||
|
|
{20, 10, 7}, //rm imu
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
for (uint8_t i = 0; i < ERROR_LIST_LENGHT; i++)
|
|||
|
|
{
|
|||
|
|
error_list[i].set_offline_time = set_item[i][0];
|
|||
|
|
error_list[i].set_online_time = set_item[i][1];
|
|||
|
|
error_list[i].priority = set_item[i][2];
|
|||
|
|
error_list[i].data_is_error_fun = NULL;
|
|||
|
|
error_list[i].solve_lost_fun = NULL;
|
|||
|
|
error_list[i].solve_data_error_fun = NULL;
|
|||
|
|
|
|||
|
|
error_list[i].enable = 1;
|
|||
|
|
error_list[i].error_exist = 1;
|
|||
|
|
error_list[i].is_lost = 1;
|
|||
|
|
error_list[i].data_is_error = 1;
|
|||
|
|
error_list[i].frequency = 0.0f;
|
|||
|
|
error_list[i].new_time = time;
|
|||
|
|
error_list[i].last_time = time;
|
|||
|
|
error_list[i].lost_time = time;
|
|||
|
|
error_list[i].work_time = time;
|
|||
|
|
}
|
|||
|
|
// error_list[NUC_TOE].solve_lost_fun = NUC_HandleOffline;
|
|||
|
|
// error_list[DBUSTOE].dataIsErrorFun = RC_data_is_error;
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|