Simulink - Arduino to read out sparkfun 9dof IMU returns zero
6 次查看(过去 30 天)
显示 更早的评论
Dear helpful simulink/c-code guru,
Currently I am trying to get my 9dof IMU working in simulink.
Searching for a solution, I found that the S-function builder can be used to use the C code directly from the library files when running in external mode directly on the arduino.
The used library does work in the arduino INO, but I would like to get the signals in my simulink model to use in a feedback loop for a drone.
When running in external mode, the output from the IMU is zero.
All libraries are in the same folder and seem to be working (in attachment a zip of my work folder with block and libraries).
I suppose the fault is located in the conversion between data types:
in the library uint8/uint16 signals are read out from the IMU, but in simulink these are "I don't know what" but I tried every possible data type without success.
Anyone who could help me out of this problem?
Thanks in advance!
Gert
/*
* Include Files
*
*/
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#ifndef MATLAB_MEX_FILE
#define ARDUINO 100 % include the libraries which are working in arduino INO
#include <Arduino.h>
#include "Wire.h"
#include "Wire.cpp"
#include "SPI.h"
#include "LSM9DS1_Registers.h"
#include "LSM9DS1_Types.h"
#include "SparkFunLSM9DS1.h"
#include "SparkFunLSM9DS1.cpp"
#include "twi.h"
#include "twi.c"
LSM9DS1 imu;
///////////////////////
// Example I2C Setup //
///////////////////////
// SDO_XM and SDO_G are both pulled high, so our addresses are:
#define LSM9DS1_M 0x1E // Would be 0x1C if SDO_M is LOW
#define LSM9DS1_AG 0x6B // Would be 0x6A if SDO_AG is LOW
#define DECLINATION 1.86 // Declination (degrees) in 50.8,5.4
#endif
/* %%%-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 */
/* extern double func(double a); */
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
/*
* Output function
*
*/
extern "C" void sf_SparkFunLSM9DS1_read_Outputs_wrapper(real_T *gx,
real_T *gy,
real_T *gz,
real_T *ax,
real_T *ay,
real_T *az,
real_T *mx,
real_T *my,
real_T *mz,
real_T *roll_a_n,
real_T *pitch_a_n,
const real_T *xD)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
/* This sample sets the output equal to the input
y0[0] = u0[0];
For complex signals use: y0[0].re = u0[0].re;
y0[0].im = u0[0].im;
y1[0].re = u1[0].re;
y1[0].im = u1[0].im;
*/
if (xD[0] == 1 ) {
#ifndef MATLAB_MEX_FILE
imu.settings.device.commInterface = IMU_MODE_I2C;
imu.settings.device.mAddress = LSM9DS1_M;
imu.settings.device.agAddress = LSM9DS1_AG;
// Update the sensor values whenever new data is available
if ( imu.gyroAvailable() )
{
// To read from the gyroscope, first call the
// readGyro() function. When it exits, it'll update the
// gx, gy, and gz variables with the most current data.
imu.readGyro();
gy[0]=5; % for testing OK
}
if ( imu.accelAvailable() )
{
// To read from the accelerometer, first call the
// readAccel() function. When it exits, it'll update the
// ax, ay, and az variables with the most current data.
imu.readAccel();
ax[0]=5; % for testing OK
}
if ( imu.magAvailable() )
{
// To read from the magnetometer, first call the
// readMag() function. When it exits, it'll update the
// mx, my, and mz variables with the most current data.
imu.readMag();
mx[0]=5; % testing, OK
}
// int ii=0;
// while (ii < 1000){
gx[0] = 5.5; % for testing, OK
// ii++;
// }
// gx[0] = imu.gx;
// gy[0] = imu.gy;
gz[0] = imu.gz;
// ax[0] = imu.ax;
// ay[0] = imu.ay;
// az[0] = imu.az;
mx[0] = imu.mx;
my[0] = imu.my;
mz[0] = imu.mz;
float ax_t = imu.calcAccel(imu.ax);
float ay_t = imu.calcAccel(imu.ay);
float az_t = imu.calcAccel(imu.az);
ax[0] = ax_t;
ay[0] = ay_t;
az[0] = az_t;
roll_a_n[0] = atan2(ay_t, az_t)*180/PI;
pitch_a_n[0] = atan2(-ax_t, sqrt(ay_t * ay_t + az_t * az_t))*180/PI;
// float yaw = atan2(sqrt(ax * ax + ay*ay),az)*180/PI;
#endif
}
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Updates function
*
*/
extern "C" void sf_SparkFunLSM9DS1_read_Update_wrapper(real_T *gx,
real_T *gy,
real_T *gz,
real_T *ax,
real_T *ay,
real_T *az,
real_T *mx,
real_T *my,
real_T *mz,
real_T *roll_a_n,
real_T *pitch_a_n,
real_T *xD)
{
/* %%%-SFUNWIZ_wrapper_Update_Changes_BEGIN --- EDIT HERE TO _END */
/*
* Code example
* xD[0] = u0[0];
*/
if (xD[0]!=1) {
# ifndef MATLAB_MEX_FILE
imu.settings.device.commInterface = IMU_MODE_I2C;
imu.settings.device.mAddress = LSM9DS1_M;
imu.settings.device.agAddress = LSM9DS1_AG;
#endif
gx[0]=2; % for testing, this goes correctly to output
gy[0]=3;
xD[0] = 1;
}
/* %%%-SFUNWIZ_wrapper_Update_Changes_END --- EDIT HERE TO _BEGIN */
}
example from library SparkFunLSM9DS1.cpp
void LSM9DS1::readAccel()
{
uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp
if ( xgReadBytes(OUT_X_L_XL, temp, 6) == 6 ) // Read 6 bytes, beginning at OUT_X_L_XL
{
ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax
ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
az = (temp[5] << 8) | temp[4]; // Store z-axis values into az
if (_autoCalc)
{
ax -= aBiasRaw[X_AXIS];
ay -= aBiasRaw[Y_AXIS];
az -= aBiasRaw[Z_AXIS];
}
}
}
info from header SparkFunLSM9DS1.h:
class LSM9DS1
{
public:
IMUSettings settings;
// We'll store the gyro, accel, and magnetometer readings in a series of
// public class variables. Each sensor gets three variables -- one for each
// axis. Call readGyro(), readAccel(), and readMag() first, before using
// these variables!
// These values are the RAW signed 16-bit readings from the sensors.
int16_t gx, gy, gz; // x, y, and z axis readings of the gyroscope
int16_t ax, ay, az; // x, y, and z axis readings of the accelerometer
int16_t mx, my, mz; // x, y, and z axis readings of the magnetometer
int16_t temperature; // Chip temperature
float gBias[3], aBias[3], mBias[3];
int16_t gBiasRaw[3], aBiasRaw[3], mBiasRaw[3];
0 个评论
回答(1 个)
Gayatri Menon
2022-3-13
Hey
LSM9DS1 block in Simulink can be used to read the data from LSM9DS1 sensor
Please see the link for more info
Hope this helps
Thanks
Gayatri
0 个评论
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Run on Target Hardware 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!