/*
 * 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 */
}