组合导航系统之一：RT-Thread操作系统
描述
组合导航系统是裸机运行还是跑操作系统，其实是仁者见仁，智者见智。笔者最开始是忠实的无操作系统支持者，认为组合导航这种实时及时间性较强的场合不应用操作系统。再实时的系统也会有延时，不如直观上利用中断方式进行前后台计算来的快。
但随着任务量的攀升和人工智能的高速发展，遂开始研究了操作系统加持下的组合导航。加上操作系统后，才意识到自己之前的愚钝。于是自己动手写了个简单的操作系统NavOS，在MIT大神的帮助下，完成了1.0和2.0，无奈维护成本太高，最终放弃。遂拥抱RT-Thread，自己不用维护，省心省力。
任务分解
- 任务梳理：
#define TASK_SENSOR_READ 0 //传感器读取
#define TASK_DEV_CALI 1 //标定补偿
#define TASK_DATA_ANALY 2 //数据预处理
#define TASK_GNSS_SOL 3 //卫星解析
#define TASK_TRANSMIT_DATA 4 //数据发送
#define TASK_COMMAND_RESOLVE 5 //报文解析
#define TASK_COMMAND_SEND 6
#define TASK_INS_INIT 7 //algorithm initial
#define TASK_MATRIX_INIT 8
#define TASK_KALMAN_INIT 9
#define TASK_EKF_INIT 10
#define TASK_ALIGN 11
#define TASK_INS_LOAD_DATA 12
#define TASK_INS_BUFFER_DATA 13
#define TASK_INS_CAL 14
#define TASK_EKF_CAL 15
#define TASK_EKF_COORCH 16
#define TASK_GPS_RES 17
#define TASK_GPS_INIT 18
#define TASK_SINS_MANAGE 19
- 任务初始化
Task_Functions[TASK_SENSOR_READ].id = TASK_SENSOR_READ;
Task_Functions[TASK_SENSOR_READ].name = "SensorRead";
Task_Functions[TASK_SENSOR_READ].func_p = Sensor_Original_Info.updata_func;
Task_Functions[TASK_SENSOR_READ].parameter = RT_NULL;
Task_Functions[TASK_SENSOR_READ].stack_size = 512;
Task_Functions[TASK_SENSOR_READ].priority = 2;
Task_Functions[TASK_SENSOR_READ].tick = 10;
Task_Functions[TASK_SENSOR_READ].runtime = 2;
Task_Functions[TASK_SENSOR_READ].RunType = TaskRunForever;
Task_Functions[TASK_SENSOR_READ].StateType = NormalTask;
Task_Functions[TASK_SENSOR_READ].ThreadType = ThreadStatic;
Task_Functions[TASK_DEV_CALI].id = TASK_DEV_CALI;
Task_Functions[TASK_DEV_CALI].name = "Compen";
Task_Functions[TASK_DEV_CALI].func_p = &Dev_Compensation;
Task_Functions[TASK_DEV_CALI].parameter = RT_NULL;
Task_Functions[TASK_DEV_CALI].stack_size = 256;
Task_Functions[TASK_DEV_CALI].priority = 3;
Task_Functions[TASK_DEV_CALI].tick = 20;
Task_Functions[TASK_DEV_CALI].runtime = 5;
Task_Functions[TASK_DEV_CALI].RunType = TaskRunForever;
Task_Functions[TASK_DEV_CALI].StateType = NormalTask;
Task_Functions[TASK_DEV_CALI].ThreadType = ThreadStatic;
Task_Functions[TASK_TRANSMIT_DATA].id = TASK_TRANSMIT_DATA;
Task_Functions[TASK_TRANSMIT_DATA].name = "Trans";
Task_Functions[TASK_TRANSMIT_DATA].func_p = &Transmit_Data_To_PC;
Task_Functions[TASK_TRANSMIT_DATA].parameter = RT_NULL;
Task_Functions[TASK_TRANSMIT_DATA].stack_size = 512;
Task_Functions[TASK_TRANSMIT_DATA].priority = 0;
Task_Functions[TASK_TRANSMIT_DATA].tick = 20;
if(Task_Functions[TASK_TRANSMIT_DATA].runtime == 5){
Task_Functions[TASK_TRANSMIT_DATA].runtime = 5;
}
Task_Functions[TASK_TRANSMIT_DATA].RunType = TaskRunForever;
Task_Functions[TASK_TRANSMIT_DATA].StateType = NormalTask;
Task_Functions[TASK_TRANSMIT_DATA].ThreadType = ThreadStatic;
Task_Functions[TASK_COMMAND_RESOLVE].id = TASK_COMMAND_RESOLVE;
Task_Functions[TASK_COMMAND_RESOLVE].name = "command";
Task_Functions[TASK_COMMAND_RESOLVE].func_p = &Command_Resolve;
Task_Functions[TASK_COMMAND_RESOLVE].parameter = RT_NULL;
Task_Functions[TASK_COMMAND_RESOLVE].stack_size = 2048;
Task_Functions[TASK_COMMAND_RESOLVE].priority = 0;
Task_Functions[TASK_COMMAND_RESOLVE].tick = 5000;
Task_Functions[TASK_COMMAND_RESOLVE].runtime = 5;
Task_Functions[TASK_COMMAND_RESOLVE].RunType = TaskRunInCondition;
Task_Functions[TASK_COMMAND_RESOLVE].StateType = SuspendTask;
Task_Functions[TASK_COMMAND_RESOLVE].ThreadType = ThreadStatic;
Task_Functions[TASK_GPS_RES].id = TASK_GPS_RES;
Task_Functions[TASK_GPS_RES].name = "GpsRes";
Task_Functions[TASK_GPS_RES].func_p = GNSS_Func_Par.Res;
Task_Functions[TASK_GPS_RES].parameter = RT_NULL;
Task_Functions[TASK_GPS_RES].stack_size = 512;
Task_Functions[TASK_GPS_RES].priority = 10;
Task_Functions[TASK_GPS_RES].tick = 10;
Task_Functions[TASK_GPS_RES].runtime = 50;
Task_Functions[TASK_GPS_RES].RunType = TaskRunInCondition;
Task_Functions[TASK_GPS_RES].StateType = SuspendTask;
Task_Functions[TASK_GPS_RES].ThreadType = ThreadStatic;
Task_Functions[TASK_GPS_INIT].id = TASK_GPS_INIT;
Task_Functions[TASK_GPS_INIT].name = "GpsInit";
Task_Functions[TASK_GPS_INIT].func_p = GNSS_Func_Par.Init;
Task_Functions[TASK_GPS_INIT].parameter = RT_NULL;
Task_Functions[TASK_GPS_INIT].stack_size = 128;
Task_Functions[TASK_GPS_INIT].priority = 11;
Task_Functions[TASK_GPS_INIT].tick = 100;
Task_Functions[TASK_GPS_INIT].runtime = 1;
Task_Functions[TASK_GPS_INIT].RunType = TaskRunOnceTime;
Task_Functions[TASK_GPS_INIT].StateType = NormalTask;
Task_Functions[TASK_GPS_INIT].ThreadType = ThreadStatic;
Task_Functions[TASK_INS_INIT].id = TASK_INS_INIT;
Task_Functions[TASK_INS_INIT].name = "InsInit";
Task_Functions[TASK_INS_INIT].func_p = INS_Nav_Par.Init;
Task_Functions[TASK_INS_INIT].parameter = RT_NULL;
Task_Functions[TASK_INS_INIT].stack_size = 128;
Task_Functions[TASK_INS_INIT].priority = 0;
Task_Functions[TASK_INS_INIT].tick = 100;
Task_Functions[TASK_INS_INIT].runtime = 1;
Task_Functions[TASK_INS_INIT].RunType = TaskRunOnceTime;
Task_Functions[TASK_INS_INIT].StateType = NormalTask;
Task_Functions[TASK_INS_INIT].ThreadType = ThreadStatic;
Task_Functions[TASK_MATRIX_INIT].id = TASK_MATRIX_INIT;
Task_Functions[TASK_MATRIX_INIT].name = "MatInit";
Task_Functions[TASK_MATRIX_INIT].func_p = Mat_Cal_Info.Init;
Task_Functions[TASK_MATRIX_INIT].parameter = RT_NULL;
Task_Functions[TASK_MATRIX_INIT].stack_size = 128;
Task_Functions[TASK_MATRIX_INIT].priority = 0;
Task_Functions[TASK_MATRIX_INIT].tick = 100;
Task_Functions[TASK_MATRIX_INIT].runtime = 1;
Task_Functions[TASK_MATRIX_INIT].RunType = TaskRunOnceTime;
Task_Functions[TASK_MATRIX_INIT].StateType = NormalTask;
Task_Functions[TASK_MATRIX_INIT].ThreadType = ThreadStatic;
Task_Functions[TASK_KALMAN_INIT].id = TASK_KALMAN_INIT;
Task_Functions[TASK_KALMAN_INIT].name = "KalInit";
Task_Functions[TASK_KALMAN_INIT].func_p = Kalman_Cal_Func.Init;
Task_Functions[TASK_KALMAN_INIT].parameter = RT_NULL;
Task_Functions[TASK_KALMAN_INIT].stack_size = 128;
Task_Functions[TASK_KALMAN_INIT].priority = 0;
Task_Functions[TASK_KALMAN_INIT].tick = 100;
Task_Functions[TASK_KALMAN_INIT].runtime = 1;
Task_Functions[TASK_KALMAN_INIT].RunType = TaskRunOnceTime;
Task_Functions[TASK_KALMAN_INIT].StateType = NormalTask;
Task_Functions[TASK_KALMAN_INIT].ThreadType = ThreadStatic;
Task_Functions[TASK_EKF_INIT].id = TASK_EKF_INIT;
Task_Functions[TASK_EKF_INIT].name = "EkfInit";
Task_Functions[TASK_EKF_INIT].func_p = EKFInit;
Task_Functions[TASK_EKF_INIT].parameter = RT_NULL;
Task_Functions[TASK_EKF_INIT].stack_size = 128;
Task_Functions[TASK_EKF_INIT].priority = 0;
Task_Functions[TASK_EKF_INIT].tick = 100;
Task_Functions[TASK_EKF_INIT].runtime = 1;
Task_Functions[TASK_EKF_INIT].RunType = TaskRunOnceTime;
Task_Functions[TASK_EKF_INIT].StateType = NormalTask;
Task_Functions[TASK_EKF_INIT].ThreadType = ThreadStatic;
Task_Functions[TASK_ALIGN].id = TASK_ALIGN;
Task_Functions[TASK_ALIGN].name = "AlignMent";
Task_Functions[TASK_ALIGN].func_p = INS_Nav_Par.StaAlign;
Task_Functions[TASK_ALIGN].parameter = RT_NULL;
Task_Functions[TASK_ALIGN].stack_size = 128;
Task_Functions[TASK_ALIGN].priority = 3;
Task_Functions[TASK_ALIGN].tick = 100;
Task_Functions[TASK_ALIGN].runtime = 5;
Task_Functions[TASK_ALIGN].RunType = TaskRunForever;
Task_Functions[TASK_ALIGN].StateType = NormalTask;
Task_Functions[TASK_ALIGN].ThreadType = ThreadStatic;
Task_Functions[TASK_INS_LOAD_DATA].id = TASK_INS_LOAD_DATA;
Task_Functions[TASK_INS_LOAD_DATA].name = "InsLoad";
Task_Functions[TASK_INS_LOAD_DATA].func_p = INS_Nav_Par.LoadData;
Task_Functions[TASK_INS_LOAD_DATA].parameter = RT_NULL;
Task_Functions[TASK_INS_LOAD_DATA].stack_size = 128;
Task_Functions[TASK_INS_LOAD_DATA].priority = 4;
Task_Functions[TASK_INS_LOAD_DATA].tick = 100;
Task_Functions[TASK_INS_LOAD_DATA].runtime = 5;
Task_Functions[TASK_INS_LOAD_DATA].RunType = TaskRunForever;
Task_Functions[TASK_INS_LOAD_DATA].StateType = NormalTask;
Task_Functions[TASK_INS_LOAD_DATA].ThreadType = ThreadStatic;
Task_Functions[TASK_INS_BUFFER_DATA].id = TASK_INS_BUFFER_DATA;
Task_Functions[TASK_INS_BUFFER_DATA].name = "InsBuff";
Task_Functions[TASK_INS_BUFFER_DATA].func_p = INS_Nav_Par.BufferData;
Task_Functions[TASK_INS_BUFFER_DATA].parameter = RT_NULL;
Task_Functions[TASK_INS_BUFFER_DATA].stack_size = 128;
Task_Functions[TASK_INS_BUFFER_DATA].priority = 4;
Task_Functions[TASK_INS_BUFFER_DATA].tick = 100;
Task_Functions[TASK_INS_BUFFER_DATA].runtime = 5;
Task_Functions[TASK_INS_BUFFER_DATA].RunType = TaskRunForever;
Task_Functions[TASK_INS_BUFFER_DATA].StateType = NormalTask;
Task_Functions[TASK_INS_BUFFER_DATA].ThreadType = ThreadStatic;
Task_Functions[TASK_INS_CAL].id = TASK_INS_CAL;
Task_Functions[TASK_INS_CAL].name = "InsCal";
Task_Functions[TASK_INS_CAL].func_p = INS_Nav_Par.INS_Cal;
Task_Functions[TASK_INS_CAL].parameter = RT_NULL;
Task_Functions[TASK_INS_CAL].stack_size = 128;
Task_Functions[TASK_INS_CAL].priority = 4;
Task_Functions[TASK_INS_CAL].tick = 100;
Task_Functions[TASK_INS_CAL].runtime = 5;
Task_Functions[TASK_INS_CAL].RunType = TaskRunForever;
Task_Functions[TASK_INS_CAL].StateType = SuspendTask;
Task_Functions[TASK_INS_CAL].ThreadType = ThreadStatic;
Task_Functions[TASK_EKF_COORCH].id = TASK_INS_CAL;
Task_Functions[TASK_EKF_COORCH].name = "Ekfcoor";
Task_Functions[TASK_EKF_COORCH].func_p = EKFCoorChange;
Task_Functions[TASK_EKF_COORCH].parameter = RT_NULL;
Task_Functions[TASK_EKF_COORCH].stack_size = 128;
Task_Functions[TASK_EKF_COORCH].priority = 5;
Task_Functions[TASK_EKF_COORCH].tick = 10;
Task_Functions[TASK_EKF_COORCH].runtime = 5;
Task_Functions[TASK_EKF_COORCH].RunType = TaskRunForever;
Task_Functions[TASK_EKF_COORCH].StateType = NormalTask;
Task_Functions[TASK_EKF_COORCH].ThreadType = ThreadStatic;
Task_Functions[TASK_EKF_CAL].id = TASK_EKF_CAL;
Task_Functions[TASK_EKF_CAL].name = "EkfCal";
Task_Functions[TASK_EKF_CAL].func_p = EKFCal;
Task_Functions[TASK_EKF_CAL].parameter = RT_NULL;
Task_Functions[TASK_EKF_CAL].stack_size = 128;
Task_Functions[TASK_EKF_CAL].priority = 5;
Task_Functions[TASK_EKF_CAL].tick = 100;
Task_Functions[TASK_EKF_CAL].runtime = 5;
Task_Functions[TASK_EKF_CAL].RunType = TaskRunForever;
Task_Functions[TASK_EKF_CAL].StateType = SuspendTask;
Task_Functions[TASK_EKF_CAL].ThreadType = ThreadStatic;
Task_Functions[TASK_SINS_MANAGE].id = TASK_INS_CAL;
Task_Functions[TASK_SINS_MANAGE].name = "sInsMana";
Task_Functions[TASK_SINS_MANAGE].func_p = SINS_Manage;
Task_Functions[TASK_SINS_MANAGE].parameter = RT_NULL;
Task_Functions[TASK_SINS_MANAGE].stack_size = 128;
Task_Functions[TASK_SINS_MANAGE].priority = 12;
Task_Functions[TASK_SINS_MANAGE].tick = 100;
Task_Functions[TASK_SINS_MANAGE].runtime = 5;
Task_Functions[TASK_SINS_MANAGE].RunType = TaskRunForever;
Task_Functions[TASK_SINS_MANAGE].StateType = NormalTask;
Task_Functions[TASK_SINS_MANAGE].ThreadType = ThreadStatic;
- 任务启动
Task_Seque_Par.Create(TASK_SENSOR_READ);
Task_Seque_Par.Create(TASK_DEV_CALI);
Task_Seque_Par.Create(TASK_TRANSMIT_DATA);
Task_Seque_Par.Create(TASK_COMMAND_RESOLVE);
Task_Seque_Par.Create(TASK_GPS_RES);
Task_Seque_Par.Create(TASK_GPS_INIT);
Task_Seque_Par.Create(TASK_INS_INIT);
Task_Seque_Par.Create(TASK_MATRIX_INIT);
Task_Seque_Par.Create(TASK_KALMAN_INIT);
Task_Seque_Par.Create(TASK_EKF_INIT);
Task_Seque_Par.Create(TASK_ALIGN);
Task_Seque_Par.Create(TASK_INS_LOAD_DATA);
Task_Seque_Par.Create(TASK_INS_BUFFER_DATA);
Task_Seque_Par.Create(TASK_INS_CAL);
Task_Seque_Par.Create(TASK_EKF_COORCH);
Task_Seque_Par.Create(TASK_EKF_CAL);
Task_Seque_Par.Create(TASK_SINS_MANAGE);
Task_Seque_Par.Activate(TASK_SENSOR_READ);
Task_Seque_Par.Activate(TASK_DEV_CALI);
Task_Seque_Par.Activate(TASK_TRANSMIT_DATA);
Task_Seque_Par.Activate(TASK_COMMAND_RESOLVE);
Task_Seque_Par.Activate(TASK_GPS_RES);
Task_Seque_Par.Activate(TASK_GPS_INIT);
Task_Seque_Par.Activate(TASK_INS_INIT);
Task_Seque_Par.Activate(TASK_EKF_INIT);
Task_Seque_Par.Activate(TASK_MATRIX_INIT);
Task_Seque_Par.Activate(TASK_KALMAN_INIT);
Task_Seque_Par.Activate(TASK_ALIGN);
Task_Seque_Par.Activate(TASK_INS_LOAD_DATA);
Task_Seque_Par.Activate(TASK_INS_BUFFER_DATA);
Task_Seque_Par.Activate(TASK_INS_CAL);
Task_Seque_Par.Activate(TASK_EKF_CAL);
Task_Seque_Par.Activate(TASK_EKF_COORCH);
Task_Seque_Par.Activate(TASK_SINS_MANAGE);
任务调度
void SINS_Manage(void){
static uint8_t RunFlag=false;
if(RunFlag==false){
if(System_Work_Info.System_Time>10000.0f){
RunFlag=true;
Task_Seque_Par.Suspend(TASK_ALIGN);
Task_Seque_Par.Resume(TASK_INS_CAL);
Task_Seque_Par.Resume(TASK_EKF_CAL);
Task_Seque_Par.Suspend(TASK_SINS_MANAGE);
}else{
Dev_Set_Info.WorkMode=3; ///< 初始对准模式
}
}
}
未完待续…
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
