matt
140 lines
2.8 KiB
C
140 lines
2.8 KiB
C
|
|
/*
|
|
* 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
|
|
#include "uart_fifos.h"
|
|
#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[2];
|
|
|
|
void initialize_gps(uint8_t id)
|
|
{
|
|
switch (id)
|
|
{
|
|
case 0:
|
|
if (!gps_inited[0])
|
|
{
|
|
#ifdef HAL_IMPL
|
|
gps_inited[0] = 1;
|
|
#endif
|
|
}
|
|
break;
|
|
|
|
case 1:
|
|
if (!gps_inited[1])
|
|
{
|
|
#ifdef HAL_IMPL
|
|
gps_inited[1] = 1;
|
|
#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:
|
|
if (gps_inited[id[0]])
|
|
{
|
|
#ifdef HAL_IMPL
|
|
gps->latitude = g_Ublox.LatDeg*1e-7;
|
|
gps->longitude = g_Ublox.LonDeg*1e-7;
|
|
gps->altitude = g_Ublox.msl*0.001f;
|
|
|
|
gps->vel_north = g_Ublox.vn*0.001f;
|
|
gps->vel_east = g_Ublox.ve*0.001f;
|
|
gps->vel_down = g_Ublox.vd*0.001f;
|
|
|
|
gps->heading = 0; ///< Unit is radian, valid when fixtype_att>1
|
|
gps->pitch = 0; ///< Unit is radian, valid when fixtype_att>1
|
|
|
|
gps->h_acc = g_Ublox.hacc*0.001f;
|
|
gps->v_acc = g_Ublox.vacc*0.001f;
|
|
gps->vel_acc = g_Ublox.sacc*0.001f;
|
|
gps->att_acc = g_Ublox.cacc/57.3f;
|
|
|
|
gps->TOW = g_Ublox.towms;
|
|
gps->WN = (g_Ublox.utc.year%100)*10000+g_Ublox.utc.month*100+g_Ublox.utc.day;
|
|
gps->seq = g_Ublox.seq;
|
|
gps->seq_att = 0;
|
|
gps->HDOP = g_Ublox.pdop*100; //RTK传过来的只有pdop
|
|
gps->VDOP = g_Ublox.pdop*100; //RTK传过来的只有pdop
|
|
if (g_Ublox.RtkType & 0x01)
|
|
{
|
|
gps->fixtype = g_Ublox.GpsType;
|
|
}
|
|
else
|
|
{
|
|
gps->fixtype = 0;
|
|
}
|
|
gps->fixtype_att = 0; //普通GPS没意义 赋值0
|
|
gps->satnum = g_Ublox.SvNum;
|
|
|
|
ErrorCode[0] = 0;
|
|
#endif
|
|
}
|
|
else
|
|
ErrorCode[0] = 1;
|
|
break;
|
|
|
|
case 1:
|
|
if (gps_inited[1])
|
|
{
|
|
#ifdef HAL_IMPL
|
|
ErrorCode[0] = 1;
|
|
#endif
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
|
|
}
|
|
|
|
|