hm/slhal
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix to select next good gps if not present

Browse Source
This commit is contained in:
matt
2020-10-14 10:07:29 +08:00
parent ec54a943e1
commit 4dc528cefd
1 changed files with 40 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EF3Lite/hal_gps_wrapper.c
+40 -15
View File
@@ -152,6 +152,8 @@ void hal_gps_Outputs_wrapper(HAL_GPS_SI_t *gps,
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
ErrorCode[0] = -1;
uint8_T gps_id;
int cnt;
switch (id[0])
{
case 0: //普通GPS通过id[0]来区分index
@@ -160,16 +162,34 @@ ErrorCode[0] = -1;
if (gps_inited[id[0]])
{
#ifdef HAL_IMPL
gps_id = id[0];
cnt = 0;
while (cnt < 3 && gps_info.gps_info_s[gps_id].ExternJudge != GPS_JUDGE_OK)
{
if (gps_id == 0)
{
gps_id = 1;
}
else if (gps_id == 1)
{
gps_id = 2;
}
else if (gps_id == 2)
{
gps_id = 0;
}
++cnt;
}
// 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->latitude = gps_info.gps_info_s[gps_id].LatDeg; ///< Unit is degree
gps->longitude = gps_info.gps_info_s[gps_id].LonDeg; ///< Unit is degree
gps->altitude = gps_info.gps_info_s[gps_id].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->vel_north = gps_info.gps_info_s[gps_id].VelNorth; ///< Unit is meter per second, in NED axes
gps->vel_east = gps_info.gps_info_s[gps_id].VelEast; ///< Unit is meter per second, in NED axes
gps->vel_down = gps_info.gps_info_s[gps_id].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->heading = gps_info.gps_info_s[gps_id].HeadingMotionDeg * D2R; ///< Unit is radian, valid when fixtype_att>1
gps->pitch = 0; ///< Unit is radian, valid when fixtype_att>1
/** 待确定
@@ -179,25 +199,27 @@ ErrorCode[0] = -1;
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
SecondFromUTC = GetMilSecondTime(gps_info.gps_info_s[gps_id].year, gps_info.gps_info_s[gps_id].month, gps_info.gps_info_s[gps_id].day,
gps_info.gps_info_s[gps_id].hour, gps_info.gps_info_s[gps_id].min, gps_info.gps_info_s[gps_id].isec);
gps->TOW = (SecondFromUTC % SECOND_OF_WEEK)*1000 + gps_info.gps_info_s[gps_id].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*100; //RTK传过来的只有pdop
gps->VDOP = gps_info.gps_info_s[id[0]].pdop*100; //RTK传过来的只有pdop
gps->fixtype = gps_info.gps_info_s[id[0]].FixType;
gps->HDOP = gps_info.gps_info_s[gps_id].pdop*100; //RTK传过来的只有pdop
gps->VDOP = gps_info.gps_info_s[gps_id].pdop*100; //RTK传过来的只有pdop
gps->fixtype = gps_info.gps_info_s[gps_id].FixType;
gps->fixtype_att = 0; //普通GPS没意义 赋值0
gps->satnum = gps_info.gps_info_s[id[0]].SvNum;
gps->satnum = gps_info.gps_info_s[gps_id].SvNum;
ErrorCode[0] = 0;
#endif
}
else
{
initialize_gps(id[0]);
ErrorCode[0] = 1;
}
break;
case 4:
@@ -240,7 +262,10 @@ ErrorCode[0] = -1;
ErrorCode[0] = 0;
}
else
{
initialize_gps(id[0]);
ErrorCode[0] = 1;
}
#endif
}
break;