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

Question

Gert Wouters 2020-2-2

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/503237-simulink-arduino-to-read-out-sparkfun-9dof-imu-returns-zero

回答： Gayatri Menon 2022-3-13

9dof_sensor_return0.zip

在 MATLAB Online 中打开

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];
 
  