/*
 * 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
/* TODO include files */
#include "fuckQvar.h"
uint32_t g_ins_seq = 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 ins_inited[1];

void initialize_ins(uint8_t id)
{
  switch (id)
  {
  case 0:
    if (!ins_inited[0])
    {
#ifdef HAL_IMPL
      // TODO initialize GPS No.0
      ins_inited[0] = 1;
#endif
    }
    break;
  default:
    break;
  }
}
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */

/*
 * Start function
 *
 */
void hal_ins_Start_wrapper(const uint8_T *id, const int_T p_width0)
{
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
initialize_ins(id[0]);
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
 * Output function
 *
 */
void hal_ins_Outputs_wrapper(HAL_INS_SI_t *ins,
			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 (ins_inited[0])
    {
#ifdef HAL_IMPL
      // TODO read pressure
      ins->latitude = Pack_EKF[EKF_for_FC].alldata.gps_latti;  ///< Unit is degree
      ins->longitude = Pack_EKF[EKF_for_FC].alldata.gps_longi;
      ins->altitude = Pack_EKF[EKF_for_FC].alldata.gps_high;

      ins->vel_north = Pack_EKF[EKF_for_FC].xEKF[3];  ///< Unit is meter per second, in NED axes
      ins->vel_east = Pack_EKF[EKF_for_FC].xEKF[4];
      ins->vel_down = Pack_EKF[EKF_for_FC].xEKF[5];

      ins->roll = Pack_EKF[EKF_for_FC].alldata.true_roll/ deg;  ///< Unit is radian
      ins->pitch = Pack_EKF[EKF_for_FC].alldata.true_pitch/ deg;
      ins->yaw = Pack_EKF[EKF_for_FC].alldata.true_yaw / deg;

      ins->seq = g_ins_seq;

      /** 下面这几项待实现 */
      ins->status = 0xff;  ///< working status
                           ///< bit0 - attitude valid
                           ///< bit1 - speed valid
                           ///< bit2 - position valid
                           ///< bit7 - calibration

      ins->integration_status = 0xff; ///< integration method
                                      ///< 0 - pure Inertial mode
                                      ///< bit0(1) - GPS assist
                                      ///< bit1(2) - baro assist
                                      ///< bit2(4) - radar assist
      ins->BIT = 0;   ///< health status 0-health; otherwise-unhealth.


      ErrorCode[0] = 0;
#endif
    }
    break;
  default:
    break;
  }
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}