Hi David!
I apologize for asking questions again. I wrote a wixel app that reads values from minimu 9 v5 and sends them in array via usb com port. However, I found that reading are quite strange: accelerometer values are way too different from what they should be (1g along z axis and 0 along others). It even seems like somehow magnetometer and accelerometer values took each others place . Can you please help me to find what may cause that problem? I am afraid that the way of sending the data is not very good in my code.
Here is the wixel app code:
/** Dependencies **************************************************************/
#include <cc2511_map.h>
#include <board.h>
#include <random.h>
#include <time.h>
#include <usb.h>
#include <usb_com.h>
#include <i2c.h>
#include <stdio.h>
#define LSM6DS33 0xD6
#define CTRL1_XL 0x10
#define CTRL2_G 0x11
#define CTRL3_C 0x12
#define OUTX_L_G 0x22
#define OUTX_H_G 0x23
#define OUTY_L_G 0x24
#define OUTY_H_G 0x25
#define OUTZ_L_G 0x26
#define OUTZ_H_G 0x27
#define OUTX_L_XL 0x28
#define OUTX_H_XL 0x29
#define OUTY_L_XL 0x2A
#define OUTY_H_XL 0x2B
#define OUTZ_L_XL 0x2C
#define OUTZ_H_XL 0x2D
#define LIS3MDL 0x3C
#define CTRL1 0x20
#define CTRL2 0x21
#define CTRL3 0x22
#define CTRL4 0x23
#define CTRL5 0x24
#define OUTX_L 0x28
#define OUTX_H 0x29
#define OUTY_L 0x2A
#define OUTY_H 0x2B
#define OUTZ_L 0x2C
#define OUTZ_H 0x2D
uint8 XDATA response[18];
uint8 startReading=0;
uint8 returnResponse=0;
void updateLeds(void)
{
usbShowStatusWithGreenLed();
//LED_YELLOW(vinPowerPresent());
//LED_RED(errors);
}
void i2cMux(uint8 channel)
{
uint8 address=0xE0;
i2cStart();
i2cWriteByte(address);
i2cWriteByte(1<<channel);
i2cStop();
}
void i2cWrite(uint8 deviceAddress, uint8 regAddress, uint8 controlWord)
{
i2cStart();
i2cWriteByte(deviceAddress);
i2cWriteByte(regAddress);
i2cWriteByte(controlWord);
i2cStop();
}
uint8 i2cRead(uint8 deviceAddress, uint8 regAddress)
{
i2cStart();
i2cWriteByte(deviceAddress);
i2cWriteByte(regAddress);
i2cStart();
i2cWriteByte(deviceAddress | 1);
i2cStop();
return i2cReadByte(1);
}
void i2cInit(uint8 channel)
{
i2cMux(channel);
i2cWrite(LSM6DS33, CTRL1_XL, 0x80);
i2cWrite(LSM6DS33, CTRL2_G, 0x80);
i2cWrite(LSM6DS33, CTRL3_C, 0x04);
i2cWrite(LIS3MDL, CTRL1, 0x70);
i2cWrite(LIS3MDL, CTRL2, 0x00);
i2cWrite(LIS3MDL, CTRL3, 0x00);
i2cWrite(LIS3MDL, CTRL4, 0x0C);
}
void i2cService(uint8 channel)
{
i2cMux(channel);
response[0] = i2cRead(LSM6DS33,OUTX_L_G);
response[1] = i2cRead(LSM6DS33,OUTX_H_G);
response[2] = i2cRead(LSM6DS33,OUTY_L_G);
response[3] = i2cRead(LSM6DS33,OUTY_H_G);
response[4] = i2cRead(LSM6DS33,OUTZ_L_G);
response[5] = i2cRead(LSM6DS33,OUTZ_H_G);
response[6] = i2cRead(LSM6DS33,OUTX_L_XL);
response[7] = i2cRead(LSM6DS33,OUTX_H_XL);
response[8] = i2cRead(LSM6DS33,OUTY_L_XL);
response[9] = i2cRead(LSM6DS33,OUTY_H_XL);
response[10] = i2cRead(LSM6DS33,OUTZ_L_XL);
response[11] = i2cRead(LSM6DS33,OUTZ_H_XL);
response[12] = i2cRead(LIS3MDL,OUTX_L);
response[13] = i2cRead(LIS3MDL,OUTX_H);
response[14] = i2cRead(LIS3MDL,OUTY_L);
response[15] = i2cRead(LIS3MDL,OUTY_H);
response[16] = i2cRead(LIS3MDL,OUTZ_L);
response[17] = i2cRead(LIS3MDL,OUTZ_H);
}
void main(void)
{
systemInit();
usbInit();
i2cPinScl = 10;
i2cPinSda = 11;
i2cSetFrequency(300);
i2cSetTimeout(10);
i2cInit(0);
while (1)
{
boardService();
updateLeds();
usbComService();
if(usbComRxAvailable() && returnResponse == 0){
startReading = usbComRxReceiveByte();
}
if(startReading == 1 && returnResponse == 0)
{
i2cService(0);
returnResponse = 1;
startReading = 0;
}
if(usbComTxAvailable() && returnResponse == 1){
int i;
for(i=0;i<18;i++){
usbComTxSendByte(response[i]);
}
returnResponse=0;
}
}
}
Here is my c# code on unity:
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.IO;
using System.IO.Ports;
using System.Threading;
using System;
public class reader : MonoBehaviour {
private SerialPort port;
private string port_name;
private float GyroSens, AcceSens, MagnSens;
public MadgwickAHRS filt;
public float q0, q1, q2, q3;
public reader(){
string header_to_file = @"\\.\";
port_name=header_to_file+"COM20";
port = new SerialPort(port_name, 9600, Parity.None, 8, StopBits.One); // port settings
port.ReadTimeout = 100;
port.Handshake = Handshake.None;
port.Open();
port.DiscardOutBuffer();
port.DiscardInBuffer();
GyroSens = 8.75f / 1000f;
GyroSens = GyroSens * 3.14159265359f / 1000f;
AcceSens = 0.061f / 1000f;
MagnSens = 0.00014615609f;
filt = new MadgwickAHRS (0.01f,0.03f);
}
public string readSerial(){
float[] data = new float[9];
int value;
byte[] temp=new byte[18];
port.Write (new byte[]{1},0,1);
for (int i = 0; i < 18; i++) {
temp [i] = (byte)port.ReadByte ();
}
for (int i = 0; i < 9; i++) {
if (i >= 0 && i <= 2) {
data [i] = ((float)((short)(temp [i * 2] | temp [i * 2 + 1] << 8)));
//Debug.Log ("GYRO RAW: " + i + ", " + data [i]);
//data [i] = GyroSens*data [i];
if (data [i] < 0.01f && data [i] > -0.01f) {
data [i] = 0;
}
} else if (i >= 3 && i <= 5) {
data [i] = ((float)((short)(temp [i * 2] | temp [i * 2 + 1] << 8)));
//Debug.Log ("ACCE RAW: " + i + ", " + data [i]);
//data [i] = AcceSens * data [i];
} else {
data [i] = ((float)((short)(temp [i * 2] | temp [i * 2 + 1] << 8)));
//Debug.Log ("MAGN RAW: " + i + ", " + data [i]);
//data [i] = MagnSens * data [i];
}
}
string str = "GYRO:" + data [0] + ", " + data [1] + ", " + data [2];
str = str + "\nACCE: " + data [3] + ", " + data [4] + ", " + data [5];
str = str + "\nMAGN: " + data [6] + ", " + data [7] + ", " + data [8];
/*filt.Update (data [0], data [1], data [2], data [3], data [4], data [5], data [6], data [7], data [8]);
q0 = filt.Quaternion [0];
q1 = filt.Quaternion [1];
q2 = filt.Quaternion [2];
q3 = filt.Quaternion [3];
str = str + "\nQUAT: " + q0 + "," + q1 + ", " + q2 + ", " + q3;*/
Debug.Log (str);
System.Threading.Thread.Sleep (10);
return "";
}
}
Here is screenshot of output of unity script: