S function hangs in line while (Wire.available() < 6)
조회 수: 1 (최근 30일)
이전 댓글 표시
Hello;
I'm trying to implement a S-function block with the code of arduino to read a IMU sensor. I have the code in c++ and when I send it to the Arduino it works fine, but when I insert to the S-function block during the debug didn't receive any error, but when I turn on the Arduino it hangs.
By testing I found that the program debug in the line: mag.read(); (from the wrapper file)
Following the instruction I found that the program hangs in these lines:
#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
# include <Arduino.h>
# include "Wire.h"
# include "Wire.cpp"
# include "LIS3MDL.h"
# include "LIS3MDL.cpp"
# 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 functions
*
*/
extern "C" void lis3_Outputs_wrapper(real_T *y0,
const real_T *xD)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
if (xD[0] == 1){
# ifndef MATLAB_MEX_FILE
LIS3MDL mag;
char report[80];
mag.read();
snprintf(report, sizeof(report), "M: %6d %6d %6d",
mag.m.x, mag.m.y, mag.m.z);
Serial.println(report);
# endif
}
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Updates function
*
*/
extern "C" void lis3_Update_wrapper(real_T *y0,
real_T *xD)
{
/* %%%-SFUNWIZ_wrapper_Update_Changes_BEGIN --- EDIT HERE TO _END */
if (xD[0] != 1){
# ifndef MATLAB_MEX_FILE
LIS3MDL mag;
Serial.begin(9600);
Wire.begin();
if (!mag.init())
{
Serial.println("Failed to detect and initialize magnetometer!");
while (1);
}
mag.enableDefault();
# endif
xD[0] = 1;
}
/* %%%-SFUNWIZ_wrapper_Update_Changes_END --- EDIT HERE TO _BEGIN */
}
In the While instruction the value of while (Wire.available() < 6) always send a 1, so never pass from the while.
I can't found the origin of the problem, and it's harder to find because the code works fine directly from the INO and the C++ code.
#include <LIS3MDL.h>
#include <Wire.h>
#include <math.h>
// Defines ////////////////////////////////////////////////////////////////
// The Arduino two-wire interface uses a 7-bit number for the address,
// and sets the last bit correctly based on reads and writes
#define LIS3MDL_SA1_HIGH_ADDRESS 0b0011110
#define LIS3MDL_SA1_LOW_ADDRESS 0b0011100
#define TEST_REG_ERROR -1
#define LIS3MDL_WHO_ID 0x3D
// Constructors ////////////////////////////////////////////////////////////////
LIS3MDL::LIS3MDL(void)
{
_device = device_auto;
io_timeout = 0; // 0 = no timeout
did_timeout = false;
}
// Public Methods //////////////////////////////////////////////////////////////
// Did a timeout occur in read() since the last call to timeoutOccurred()?
bool LIS3MDL::timeoutOccurred()
{
bool tmp = did_timeout;
did_timeout = false;
return tmp;
}
void LIS3MDL::setTimeout(uint16_t timeout)
{
io_timeout = timeout;
}
uint16_t LIS3MDL::getTimeout()
{
return io_timeout;
}
bool LIS3MDL::init(deviceType device, sa1State sa1)
{
// perform auto-detection unless device type and SA1 state were both specified
if (device == device_auto || sa1 == sa1_auto)
{
// check for LIS3MDL if device is unidentified or was specified to be this type
if (device == device_auto || device == device_LIS3MDL)
{
// check SA1 high address unless SA1 was specified to be low
if (sa1 != sa1_low && testReg(LIS3MDL_SA1_HIGH_ADDRESS, WHO_AM_I) == LIS3MDL_WHO_ID)
{
sa1 = sa1_high;
if (device == device_auto) { device = device_LIS3MDL; }
}
// check SA1 low address unless SA1 was specified to be high
else if (sa1 != sa1_high && testReg(LIS3MDL_SA1_LOW_ADDRESS, WHO_AM_I) == LIS3MDL_WHO_ID)
{
sa1 = sa1_low;
if (device == device_auto) { device = device_LIS3MDL; }
}
}
// make sure device and SA1 were successfully detected; otherwise, indicate failure
if (device == device_auto || sa1 == sa1_auto)
{
return false;
}
}
_device = device;
switch (device)
{
case device_LIS3MDL:
address = (sa1 == sa1_high) ? LIS3MDL_SA1_HIGH_ADDRESS : LIS3MDL_SA1_LOW_ADDRESS;
break;
}
return true;
}
/*
Enables the LIS3MDL's magnetometer. Also:
- Selects ultra-high-performance mode for all axes
- Sets ODR (output data rate) to default power-on value of 10 Hz
- Sets magnetometer full scale (gain) to default power-on value of +/- 4 gauss
- Enables continuous conversion mode
Note that this function will also reset other settings controlled by
the registers it writes to.
*/
void LIS3MDL::enableDefault(void)
{
if (_device == device_LIS3MDL)
{
// 0x70 = 0b01110000
// OM = 11 (ultra-high-performance mode for X and Y); DO = 100 (10 Hz ODR)
writeReg(CTRL_REG1, 0x70);
// 0x00 = 0b00000000
// FS = 00 (+/- 4 gauss full scale)
writeReg(CTRL_REG2, 0x00);
// 0x00 = 0b00000000
// MD = 00 (continuous-conversion mode)
writeReg(CTRL_REG3, 0x00);
// 0x0C = 0b00001100
// OMZ = 11 (ultra-high-performance mode for Z)
writeReg(CTRL_REG4, 0x0C);
}
}
// Writes a mag register
void LIS3MDL::writeReg(uint8_t reg, uint8_t value)
{
Wire.beginTransmission(address);
Wire.write(reg);
Wire.write(value);
last_status = Wire.endTransmission();
}
// Reads a mag register
uint8_t LIS3MDL::readReg(uint8_t reg)
{
uint8_t value;
Wire.beginTransmission(address);
Wire.write(reg);
last_status = Wire.endTransmission();
Wire.requestFrom(address, (uint8_t)1);
value = Wire.read();
Wire.endTransmission();
return value;
}
// Reads the 3 mag channels and stores them in vector m
void LIS3MDL::read()
{
Wire.beginTransmission(address);
// assert MSB to enable subaddress updating
Wire.write(OUT_X_L | 0x80);
Wire.endTransmission();
Wire.requestFrom(address, (uint8_t)6);
uint16_t millis_start = millis();
while (Wire.available() < 6)
{
if (io_timeout > 0 && ((uint16_t)millis() - millis_start) > io_timeout)
{
did_timeout = true;
return;
}
}
uint8_t xlm = Wire.read();
uint8_t xhm = Wire.read();
uint8_t ylm = Wire.read();
uint8_t yhm = Wire.read();
uint8_t zlm = Wire.read();
uint8_t zhm = Wire.read();
// combine high and low bytes
m.x = (int16_t)(xhm << 8 | xlm);
m.y = (int16_t)(yhm << 8 | ylm);
m.z = (int16_t)(zhm << 8 | zlm);
}
void LIS3MDL::vector_normalize(vector<float> *a)
{
float mag = sqrt(vector_dot(a, a));
a->x /= mag;
a->y /= mag;
a->z /= mag;
}
// Private Methods //////////////////////////////////////////////////////////////
int16_t LIS3MDL::testReg(uint8_t address, regAddr reg)
{
Wire.beginTransmission(address);
Wire.write((uint8_t)reg);
if (Wire.endTransmission() != 0)
{
return TEST_REG_ERROR;
}
Wire.requestFrom(address, (uint8_t)1);
if (Wire.available())
{
return Wire.read();
}
else
{
return TEST_REG_ERROR;
}
}
Note: From the same libraries of Arduino I made other S-function blocks without problem, but this I can't make it works.
I'm attaching all the file, because there are a lot of c++ and headers for it.
I hope you can help me because I can't find the solution or any way to find it.
Thanks a lot.
댓글 수: 0
답변 (0개)
참고 항목
카테고리
Help Center 및 File Exchange에서 MATLAB Support Package for Arduino Hardware에 대해 자세히 알아보기
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!