matt
177 lines
3.6 KiB
C
177 lines
3.6 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 "spi_devs.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 imu_inited[3];
|
|
|
|
void initialize_6axis_imu(uint8_t id)
|
|
{
|
|
switch (id)
|
|
{
|
|
case 0:
|
|
if (!imu_inited[0])
|
|
{
|
|
#ifdef HAL_IMPL
|
|
// TODO initialize 6axis-IMU No.0
|
|
imu_inited[0] = 1;
|
|
#endif
|
|
}
|
|
break;
|
|
|
|
case 1:
|
|
if (!imu_inited[1])
|
|
{
|
|
#ifdef HAL_IMPL
|
|
// TODO initialize 6axis-IMU No.1
|
|
imu_inited[1] = 1;
|
|
#endif
|
|
}
|
|
break;
|
|
|
|
case 2:
|
|
if (!imu_inited[2])
|
|
{
|
|
#ifdef HAL_IMPL
|
|
// TODO initialize 6axis-IMU No.2
|
|
imu_inited[2] = 1;
|
|
#endif
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
|
|
|
|
/*
|
|
* Start function
|
|
*
|
|
*/
|
|
void hal_6axis_imu_Start_wrapper(const uint8_T *id, const int_T p_width0)
|
|
{
|
|
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
|
|
initialize_6axis_imu(id[0]);
|
|
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
|
|
}
|
|
/*
|
|
* Output function
|
|
*
|
|
*/
|
|
void hal_6axis_imu_Outputs_wrapper(HAL_acc_SI_t *acc,
|
|
HAL_gyro_SI_t *gyro,
|
|
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 (imu_inited[0])
|
|
{
|
|
#ifdef HAL_IMPL
|
|
// read 6axis-IMU
|
|
if (imu_adis1647x.success)
|
|
{
|
|
acc->a[0] = imu_adis1647x.accel[0];
|
|
acc->a[1] = imu_adis1647x.accel[1];
|
|
acc->a[2] = imu_adis1647x.accel[2];
|
|
gyro->v[0] = imu_adis1647x.gyro[0];
|
|
gyro->v[1] = imu_adis1647x.gyro[1];
|
|
gyro->v[2] = imu_adis1647x.gyro[2];
|
|
acc->seq = imu_adis1647x.seq;
|
|
gyro->seq = imu_adis1647x.seq;
|
|
acc->temperature = imu_adis1647x.temp;
|
|
gyro->temperature = imu_adis1647x.temp;
|
|
|
|
ErrorCode[0] = 0;
|
|
}
|
|
else if (imu_icm20689.success)
|
|
{
|
|
acc->a[0] = imu_icm20689.accel[0];
|
|
acc->a[1] = imu_icm20689.accel[1];
|
|
acc->a[2] = imu_icm20689.accel[2];
|
|
gyro->v[0] = imu_icm20689.gyro[0];
|
|
gyro->v[1] = imu_icm20689.gyro[1];
|
|
gyro->v[2] = imu_icm20689.gyro[2];
|
|
acc->seq = imu_icm20689.seq;
|
|
gyro->seq = imu_icm20689.seq;
|
|
acc->temperature = imu_icm20689.temp_degc;
|
|
gyro->temperature = imu_icm20689.temp_degc;
|
|
|
|
ErrorCode[0] = 0;
|
|
}
|
|
else
|
|
ErrorCode[0] = -1;
|
|
#endif
|
|
}
|
|
break;
|
|
|
|
case 1:
|
|
if (imu_inited[1])
|
|
{
|
|
#ifdef HAL_IMPL
|
|
// read 6axis-IMU
|
|
if (imu_bmi088.success)
|
|
{
|
|
acc->a[0] = imu_bmi088.accel[0];
|
|
acc->a[1] = imu_bmi088.accel[1];
|
|
acc->a[2] = imu_bmi088.accel[2];
|
|
gyro->v[0] = imu_bmi088.gyro[0];
|
|
gyro->v[1] = imu_bmi088.gyro[1];
|
|
gyro->v[2] = imu_bmi088.gyro[2];
|
|
acc->seq = imu_bmi088.accel_seq;
|
|
gyro->seq = imu_bmi088.gyro_seq;
|
|
acc->temperature = imu_bmi088.temp_degc;
|
|
gyro->temperature = imu_bmi088.temp_degc;
|
|
|
|
ErrorCode[0] = 0;
|
|
}
|
|
else
|
|
ErrorCode[0] = -1;
|
|
#endif
|
|
}
|
|
break;
|
|
|
|
case 2:
|
|
if (imu_inited[2])
|
|
{
|
|
#ifdef HAL_IMPL
|
|
// TODO read 6axis-IMU
|
|
ErrorCode[0] = -1;
|
|
#endif
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
|
|
}
|
|
|
|
|