Simulink - Arduino to read out sparkfun 9dof IMU returns zero

4 Ansichten (letzte 30 Tage)
Gert Wouters
Gert Wouters am 2 Feb. 2020
Beantwortet: Gayatri Menon am 13 Mär. 2022
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];

Antworten (1)

Gayatri Menon
Gayatri Menon am 13 Mär. 2022
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

Produkte


Version

R2019a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by