support EF3Lite
This commit is contained in:
matt
@@ -0,0 +1,253 @@
|
||||
|
||||
/*
|
||||
* Include Files
|
||||
*
|
||||
*/
|
||||
#if defined(MATLAB_MEX_FILE)
|
||||
#include "tmwtypes.h"
|
||||
#include "simstruc_types.h"
|
||||
#else
|
||||
#include "rtwtypes.h"
|
||||
#endif
|
||||
|
||||
#include "hal_api.h"
|
||||
|
||||
|
||||
/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
|
||||
#ifdef HAL_IMPL
|
||||
|
||||
uint32_t g_gps_seq = 0;
|
||||
uint32_t g_rtk_seq = 0;
|
||||
|
||||
#include "uavcan_main.h"
|
||||
/* TODO include files */
|
||||
#define PI (3.1415926535897932) /* pi */
|
||||
#define D2R (PI/180.0) /* deg to rad */
|
||||
|
||||
unsigned char DayOfMon[12] = {31,28,31,30,31,30,31,31,30,31,30,31};
|
||||
//Timestamp --> DataTime ---------------------------------------------------------------------
|
||||
#define SECOND_OF_DAY 86400 //1天的秒数
|
||||
#define SECOND_OF_WEEK (SECOND_OF_DAY*7)
|
||||
|
||||
/** UTC->总秒数 iYear是20xx年 */
|
||||
static unsigned int GetMilSecondTime(unsigned short iYear, unsigned short iMon, unsigned short iDay, unsigned short iHour, unsigned short iMin, unsigned short iSec )
|
||||
{
|
||||
unsigned short i, Cyear=0;
|
||||
unsigned int CountDay=0;
|
||||
|
||||
for(i=1980; i<iYear; i++) /* 统计1970年到本年之前的闰年数 */
|
||||
{
|
||||
if(((i%4==0) && (i%100!=0)) || (i%400==0)) Cyear++;
|
||||
}
|
||||
|
||||
CountDay = Cyear * 366 + (iYear-1980-Cyear) * 365; //本年之前的天数和
|
||||
|
||||
for(i=1; i<iMon; i++) //+本年本月前的天数和
|
||||
{
|
||||
if((i==2) && (((iYear%4==0)&&(iYear%100!=0)) || (iYear%400==0)))
|
||||
CountDay += 29;
|
||||
else
|
||||
CountDay += DayOfMon[i-1];
|
||||
}
|
||||
|
||||
CountDay += (iDay-1); //+本月的天数 至此算出了所有1970~now的天数和
|
||||
CountDay = CountDay*SECOND_OF_DAY + (unsigned long)iHour*3600 + (unsigned long)iMin*60 + iSec;
|
||||
|
||||
CountDay-=86400*6;
|
||||
return (CountDay); //返回1980.1.6年到现在的秒数
|
||||
}
|
||||
|
||||
unsigned int SecondFromUTC = 0;
|
||||
|
||||
#endif
|
||||
#include <stdint.h>
|
||||
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
|
||||
#define y_width 1
|
||||
|
||||
/*
|
||||
* Create external references here.
|
||||
*
|
||||
*/
|
||||
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
|
||||
int gps_inited[4];
|
||||
|
||||
void initialize_gps(uint8_t id) //应该定期执行，防止中途插拔GPS模块 by LQH
|
||||
{
|
||||
switch (id)
|
||||
{
|
||||
case 0:
|
||||
if (!gps_inited[0])
|
||||
{
|
||||
#ifdef HAL_IMPL
|
||||
// TODO initialize GPS No.0
|
||||
if(gps_info.num_valid >= 1)
|
||||
gps_inited[0] = 1;
|
||||
else
|
||||
gps_inited[0] = 0;
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if (!gps_inited[1])
|
||||
{
|
||||
#ifdef HAL_IMPL
|
||||
// TODO initialize GPS No.0
|
||||
if(gps_info.num_valid >= 2)
|
||||
gps_inited[1] = 1;
|
||||
else
|
||||
gps_inited[1] = 0;
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
if (!gps_inited[2])
|
||||
{
|
||||
#ifdef HAL_IMPL
|
||||
// TODO initialize GPS No.0
|
||||
if(gps_info.num_valid >= 3)
|
||||
gps_inited[2] = 1;
|
||||
else
|
||||
gps_inited[2] = 0;
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
|
||||
case 3:
|
||||
if (!gps_inited[3])
|
||||
{
|
||||
#ifdef HAL_IMPL
|
||||
// TODO initialize GPS No.0
|
||||
if(g_RtkAll.num_valid >= 1)
|
||||
gps_inited[3] = 1;
|
||||
else
|
||||
gps_inited[3] = 0;
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
|
||||
|
||||
/*
|
||||
* Start function
|
||||
*
|
||||
*/
|
||||
void hal_gps_Start_wrapper(const uint8_T *id, const int_T p_width0)
|
||||
{
|
||||
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
|
||||
initialize_gps(id[0]);
|
||||
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
|
||||
}
|
||||
/*
|
||||
* Output function
|
||||
*
|
||||
*/
|
||||
void hal_gps_Outputs_wrapper(HAL_GPS_SI_t *gps,
|
||||
int32_T *ErrorCode,
|
||||
const uint8_T *id, const int_T p_width0)
|
||||
{
|
||||
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
|
||||
ErrorCode[0] = -1;
|
||||
switch (id[0])
|
||||
{
|
||||
case 0: //普通GPS通过id[0]来区分index
|
||||
case 1:
|
||||
case 2:
|
||||
if (gps_inited[id[0]])
|
||||
{
|
||||
#ifdef HAL_IMPL
|
||||
// TODO read pressure
|
||||
gps->latitude = gps_info.gps_info_s[id[0]].LatDeg; ///< Unit is degree
|
||||
gps->longitude = gps_info.gps_info_s[id[0]].LonDeg; ///< Unit is degree
|
||||
gps->altitude = gps_info.gps_info_s[id[0]].height; ///< aititude from MSL, Unit is meter
|
||||
|
||||
gps->vel_north = gps_info.gps_info_s[id[0]].VelNorth; ///< Unit is meter per second, in NED axes
|
||||
gps->vel_east = gps_info.gps_info_s[id[0]].VelEast; ///< Unit is meter per second, in NED axes
|
||||
gps->vel_down = gps_info.gps_info_s[id[0]].VelDown; ///< Unit is meter per second, in NED axes
|
||||
|
||||
gps->heading = gps_info.gps_info_s[id[0]].HeadingMotionDeg * D2R; ///< Unit is radian, valid when fixtype_att>1
|
||||
gps->pitch = 0; ///< Unit is radian, valid when fixtype_att>1
|
||||
|
||||
/** 待确定
|
||||
float h_acc; ///< Unit is meter, Position uncertainty
|
||||
float v_acc; ///< Unit is meter, Altitude uncertainty
|
||||
float vel_acc; ///< Unit is m/s, velocity uncertainty
|
||||
float att_acc; ///< Unit is radian, attitude uncertainty when fixtype_att>1
|
||||
*/
|
||||
|
||||
SecondFromUTC = GetMilSecondTime(gps_info.gps_info_s[id[0]].year, gps_info.gps_info_s[id[0]].month, gps_info.gps_info_s[id[0]].day,
|
||||
gps_info.gps_info_s[id[0]].hour, gps_info.gps_info_s[id[0]].min, gps_info.gps_info_s[id[0]].isec);
|
||||
gps->TOW = (SecondFromUTC % SECOND_OF_WEEK)*1000 + gps_info.gps_info_s[id[0]].msec; ///< Unit is millisecond, Time of Week
|
||||
gps->WN = SecondFromUTC / SECOND_OF_WEEK; ///< Week Number of GPS
|
||||
gps->seq = g_gps_seq;
|
||||
gps->seq_att = g_gps_seq;
|
||||
gps->HDOP = gps_info.gps_info_s[id[0]].pdop; //RTK传过来的只有pdop
|
||||
gps->VDOP = gps_info.gps_info_s[id[0]].pdop; //RTK传过来的只有pdop
|
||||
gps->fixtype = gps_info.gps_info_s[id[0]].FixType;
|
||||
gps->fixtype_att = 0; //普通GPS没意义 赋值0
|
||||
gps->satnum = gps_info.gps_info_s[id[0]].SvNum;
|
||||
|
||||
|
||||
ErrorCode[0] = 0;
|
||||
#endif
|
||||
}
|
||||
else
|
||||
ErrorCode[0] = 1;
|
||||
|
||||
break;
|
||||
|
||||
case 4:
|
||||
if (gps_inited[3])
|
||||
{
|
||||
#ifdef HAL_IMPL
|
||||
// TODO read pressure
|
||||
if(g_RtkAll.num_valid > 0)
|
||||
{
|
||||
gps->latitude = g_RtkAll.rtk[0].pvt.LatDeg; ///< Unit is degree
|
||||
gps->longitude = g_RtkAll.rtk[0].pvt.LonDeg; ///< Unit is degree
|
||||
gps->altitude = g_RtkAll.rtk[0].pvt.height; ///< aititude from MSL, Unit is meter
|
||||
|
||||
gps->vel_north = g_RtkAll.rtk[0].pvt.Vn; ///< Unit is meter per second, in NED axes
|
||||
gps->vel_east = g_RtkAll.rtk[0].pvt.Ve; ///< Unit is meter per second, in NED axes
|
||||
gps->vel_down = g_RtkAll.rtk[0].pvt.Vd; ///< Unit is meter per second, in NED axes
|
||||
|
||||
gps->heading = g_RtkAll.rtk[0].att.heading* D2R/ 100.0f; ///< Unit is radian, valid when fixtype_att>1
|
||||
gps->pitch = g_RtkAll.rtk[0].att.pitch* D2R/ 100.0f; ///< Unit is radian, valid when fixtype_att>1
|
||||
|
||||
/** 待确定
|
||||
float h_acc; ///< Unit is meter, Position uncertainty
|
||||
float v_acc; ///< Unit is meter, Altitude uncertainty
|
||||
float vel_acc; ///< Unit is m/s, velocity uncertainty
|
||||
float att_acc; ///< Unit is radian, attitude uncertainty when fixtype_att>1
|
||||
*/
|
||||
|
||||
SecondFromUTC = GetMilSecondTime(g_RtkAll.rtk[0].pvt.year+2000, g_RtkAll.rtk[0].pvt.month, g_RtkAll.rtk[0].pvt.day,
|
||||
g_RtkAll.rtk[0].pvt.hour, g_RtkAll.rtk[0].pvt.min, g_RtkAll.rtk[0].pvt.sec);
|
||||
gps->TOW = (SecondFromUTC % SECOND_OF_WEEK)*1000 + g_RtkAll.rtk[0].pvt.msec; ///< Unit is millisecond, Time of Week
|
||||
gps->WN = SecondFromUTC / SECOND_OF_WEEK; ///< Week Number of GPS
|
||||
gps->seq = g_rtk_seq; //待测试一下
|
||||
gps->seq_att = g_rtk_seq;
|
||||
gps->HDOP = g_RtkAll.rtk[0].pvt.pdop; //RTK传过来的只有pdop
|
||||
gps->VDOP = g_RtkAll.rtk[0].pvt.pdop; //RTK传过来的只有pdop
|
||||
gps->fixtype = g_RtkAll.rtk[0].pvt.FixType; //存在对应关系的定义 可能要改进
|
||||
gps->fixtype_att = g_RtkAll.rtk[0].att.AttType;
|
||||
gps->satnum = g_RtkAll.rtk[0].pvt.SvNum;
|
||||
|
||||
ErrorCode[0] = 0;
|
||||
}
|
||||
else
|
||||
ErrorCode[0] = 1;
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user