matt
120 lines
2.9 KiB
C
120 lines
2.9 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
|
|
/* 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 */
|
|
}
|
|
|
|
|