Hacking in raw IMU accelleration and mag values


#1

I got this question from one of the teams working on ROS integration, but I figured I would share the approach with anyone interested:

There is no raw mag/acc data at the moment. I can tell you where you will need to modify the code if you want that data. Be warned that to suck the data through the serial point will limit how fast you can pull the raw numbers… so adding it to the current 20HZ navdata feed is no problem, but if you want to pull it faster than 20Hz to send in to a fusion algorithm… you may find that hard to do.

Basically, to add the mag and acceleration you will want to add Serial.print() statement here where it will come out at 20Hz:

https://github.com/OpenROV/openrov-software-arduino/blob/master/OpenROV/MPU9150.cpp#L211

The values you will need are the MPU.m_rawMAG[] and MPU.m_rawAccel[] vectors, you can see them being used a little above at:

https://github.com/OpenROV/openrov-software-arduino/blob/master/OpenROV/MPU9150.cpp#L123-L172

So your print statements would look something like:
Serial.print(F("ACLX=")); Serial.print(MPU.m_rawAccel[VEC3_X]); Serial.println(";");

That will get the data flowing out of the Arduino. You then need to map the Arduino data to the navdata message basically adding lines similar to this one:

and of course add them to the navdata structure here:

So it would look something like

if ('aclx' in status) {
      navdata.aclx = status.aclx;
    }

Newbie: How do I make changes in the cockpit gui? + How to remotedly connect with OpenROV
Python Notebook for controlling the ROV
Newbie: How do I make changes in the cockpit gui? + How to remotedly connect with OpenROV
#2

Hi badevguru! I have been trying to get this working and it seems that I’m running on another SW version than you did when creating this guide. I am using a 2.7 ROV, with the IMU 2.0, and SW 30.0.2. I have managed to get the accelerometers x-value in the serial stream by changing the BN055.cpp file. I now have a stream output that looks as follows

ACLX=10;

The variable here is arbitrary, but it looks correct to me. I had to do this in a different way than you did in your guide as I am using the IMU 2.0 (If someone wants to know how, I am happy to share) The question now is the .js files. I am completely green with .js, but what I have tried to do is to add the aclx variable in navdata in the OpenROVController.js file:

    var navdata = {
        roll: 0,
        pitch: 0,
        yaw: 0,
        thrust: 0,
        deapth: 0,
        hdgd: 0,
        aclx: 0
      }

Next I added the if-sentence to update the status right underneath the one for fthr:

if ('aclx' in status) {
  navdata.aclx = status.aclx;
}

When I re-upload the firmware, the aclx variable does not show up on the telemetry, and if i record some data and download it to my computer it is not in the downloaded json file either. What might I be doing wrong? When I re-open the OpenROVController.js file the changes are there. My theories are the following: 1: The telemetry data is handled somewhere else in the 30.0.2 SW version, 2: I am trying to read the data wrong or 3: I am completely off track when it comes to .js and have no idea what I am doing…

Thanks for any help! Your guides, and comments throughout the forum has helped me out a lot already, so thanks for all your work!


#3

Great to hear about your project. Be sure to keep us informed on who it goes!

Seems like you are doing everything right. After modifying the node.js code, be sure to restart the node.js process sudo /etc/init.d/openrov restart to have the changes take affect.

You should also be able to inspect the raw serial data coming from the Arduino to make sure to is printing as expected by pressing thee u key in cockpit that will open up the raw stream. (Tip: You might want to disconnect the network cable for a moment so you can see the stream as it moves fast).

If you already restored the node process and it is still not showing up let me know.


#4

Aha! Resetting the nose.js is probably what’s missing. I’ll check that out tomorrow. Thanks a bunch!!


#5

So i got a bit further now, but there are still some issues. I still have data flowing in the raw serial-monitor as you can see from this screen cap from the cockpit

and I have tried to capture the data stream in the .js file in two different ways. First you can see the initialisation of the values here:

var navdata = {
   roll: 0,
   pitch: 0,
   yaw: 0,
   thrust: 0,
   deapth: 0,
   hdgd: 0,
   ACLX: 0,
   acly: 0,
   aclz: 0
 };

I tried aclx in caps lock just to see if it made any difference, and I wrote the status update sentences as follows:

if ('ACLX' in status) {
  navdata.ACLX = status.ACLX;
}
if ('acly' in status) {
  navdata.acly = status.acly;
}
if ('aclz' in status) {
  navdata.aclz = status.aclz;
}

On my downloaded json files I now get the variable names (AXLX, acly and aclz), but they all have the value zero. It seems then that the raw serial works, the initialisation of the json variables work, but not the updating of the json variables. A picture of the .json files here:

Any ideas?


Obstacle Avoiding OpenROV using an IMU and Laser-based Vision System (LVS)
#6

Odd. I would stick some console.log statements inside the if statements to see if they are getting triggered. You should see the result in the /var/logs/openrov.log file on the beaglebone.

If you go ahead and pastebin the full file I can take a better look.


#7

Haha! as always there is some time little annoying thing that makes everything fail. I had a look at the Device.h file to see how things were structured. The problem is in the Serial.print command. In the guide it says:

Serial.print(F("ACLX=")

it should actually be a colon and not equal sign, so the correct way is:

Serial.print(F("ACLX:")

and then everything works and problem solved. Thanks so much for all the quick help! Btw, I’ll be over in the OpenROV HQ sometime next week so maybe I’ll meet you in person as well. Trident looks absolutely killer btw!


#8

Get preasure and temperature (working with 30.0.3) - OpenROVController.js:

var navdata = {
roll: 0,
pitch: 0,
yaw: 0,
thrust: 0,
deapth: 0,
hdgd: 0,
temp: 0,
pres: 0
}

Update status:

if ('temp' in status) {
  navdata.temp = status.temp;
}
if ('pres' in status) {
  navdata.pres = status.pres;
}  

But I can’t get acceleration and mag values. Any idea, tutorial? New software 30.0.3 is quite different. Also this calibration method doesn’t work (CMPU9150.cpp):

//20 hz
if( mpu_update_timer.HasElapsed( 50 ) )
{
if( MPU.read() )
{
NDataManager::m_navData.HDGD = MPU.m_fusedEulerPose[VEC3_Z] * RAD_TO_DEGREE;

  	//To convert to-180/180 to 0/360
  	if( NDataManager::m_navData.HDGD < 0 )
  	{
  		NDataManager::m_navData.HDGD += 360;
  	}
  	NDataManager::m_navData.PITC = MPU.m_fusedEulerPose[VEC3_Y] * RAD_TO_DEGREE + 2.18;
  	NDataManager::m_navData.ROLL = MPU.m_fusedEulerPose[VEC3_X] * RAD_TO_DEGREE + 2.11;
  	NDataManager::m_navData.YAW = MPU.m_fusedEulerPose[VEC3_Z] * RAD_TO_DEGREE;
  }

}


#9

Hey Brian,

I followed the steps you put up here and when I try to read the data in python through socketIO_client I get the default init values for acellerations (x, y, and z). I am using the code below

from socketIO_client import SocketIO, BaseNamespace
from datetime import datetime
import time
import os

def savedata(directory, filename, data):
try:
if not os.path.exists(directory):
os.makedirs(directory)

    datafile = open(directory + "\\" + filename, 'w+')

    try:
        datafile.writelines(data)
    finally:
        datafile.close()
except IOError as e:
    print "I/O error({0}): {1}".format(e.errno, e.strerror)

class Namespace(BaseNamespace):

def on_connect(self):
    print '[Connected]'

def on_data_response(*args):
#print ‘navdata’, args
lista = str(args[0]).split("’")

try:        
    #ComputerTime = time.strftime("%H:%M:%S") + ","
    utcTime = datetime.utcnow().strftime('%H_%M_%S_%f')[:-3] + ","
    hdgd = lista[lista.index('hdgd')+2] + ","
    pitch = lista[lista.index('pitch')+2] + ","
    roll = lista[lista.index('roll')+2] + ","
    yaw = lista[lista.index('yaw')+2] + ","
    depth = lista[lista.index('deapth')+2] + ","
    aclxx = lista[lista.index('aclx')+2] + ","
    aclyy = lista[lista.index('acly')+2] + ","
    aclzz = lista[lista.index('aclz')+2] + ","
    #temp = lista[lista.index('temp')+2] + ","
    thrust = lista[lista.index('thrust')+2]+","
    print lista
            
	  #dane.append(utcTime + hdgd + pitch + roll + yaw + depth + pres + temp + thrust + "\n")
    #dane.append(utcTime + hdgd + pitch + roll + yaw + depth + aclx + "\n")
    #dane.append(depth + aclxx + aclyy + aclzz +  "\n")
	
    print dane,'\n'#[len(dane)-1:len(dane)][:1] ,'\n'
except ValueError as e:
    print e
    dane.append(utcTime + str(e) +"\n")

def on_status_response(*args):
print 'status ‘, args
#lista = str(args[0]).split("’")
#print lista[lista.index(‘calData.accelMaxX’)+2]

#socketIO.emit('brightness_update',1)

############################################################################
print “start”

directory = ‘data’
filename = ‘DATA_’ + datetime.utcnow().strftime(’%H_%M_%S_%f’)[:-3] + '.dat’
dane = []
scanTime = 100 #seconds

socketIO = SocketIO(‘192.168.254.1’, 8080, Namespace)

#socketIO.on(‘status’, on_status_response)

socketIO.on(‘navdata’, on_data_response)

print “wait " + str(scanTime) + " seconds”

socketIO.wait(scanTime)
savedata(directory, filename, dane)

print “stop…”


I uploaded the code and restarted the server using

sudo /opt/openrov/cockpit/linux/arduino/firmware-installfromsource.sh

sudo /etc/init.d/openrov restart


My CMPU9150 is modified like this

//20 hz
if( mpu_update_timer.HasElapsed( 50 ) )
{
	    Serial.print(F("ACLX:")); Serial.print(MPU.m_rawAccel[VEC3_X]); Serial.println(";");
		Serial.print(F("ACLY=")); Serial.print(MPU.m_rawAccel[VEC3_Y]); Serial.println(";");
		Serial.print(F("ACLZ=")); Serial.print(MPU.m_rawAccel[VEC3_Z]); Serial.println(";");


	if( MPU.read() )
	{
		
		
		
		NDataManager::m_navData.HDGD = MPU.m_fusedEulerPose[VEC3_Z] * RAD_TO_DEGREE;
		
		//To convert to-180/180 to 0/360
		if( NDataManager::m_navData.HDGD < 0 )
		{
			NDataManager::m_navData.HDGD += 360;
		}

		NDataManager::m_navData.PITC = MPU.m_fusedEulerPose[VEC3_Y] * RAD_TO_DEGREE;
		NDataManager::m_navData.ROLL = MPU.m_fusedEulerPose[VEC3_X] * RAD_TO_DEGREE;
		NDataManager::m_navData.YAW = MPU.m_fusedEulerPose[VEC3_Z] * RAD_TO_DEGREE;

and my OpenROVController.js is exactly as shown above


The output Im getting is

[’{u’, ‘deapth’, ‘: u’, ‘0.02’, ‘, u’, ‘acly’, ‘: 0, u’, ‘yaw’, ‘: u’, ‘343.81’, ‘, u’, ‘aclx’, ‘: 0, u’, ‘pitch’, ‘: u’, ‘5.13’, ‘, u’, ‘thrust’, ‘: u’, ‘0.00’, ‘, u’, ‘hdgd’, ‘: u’, ‘343.81’, ‘, u’, ‘aclz’, ‘: 0, u’, ‘roll’, ‘: u’, ‘0.00’, ‘}’]
[]

[’{u’, ‘deapth’, ‘: u’, ‘-0.01’, ‘, u’, ‘acly’, ‘: 0, u’, ‘yaw’, ‘: u’, ‘343.81’, ‘, u’, ‘aclx’, ‘: 0, u’, ‘pitch’, ‘: u’, ‘5.13’, ‘, u’, ‘thrust’, ‘: u’, ‘0.00’, ‘, u’, ‘hdgd’, ‘: u’, ‘343.81’, ‘, u’, ‘aclz’, ‘: 0, u’, ‘roll’, ‘: u’, ‘0.00’, ‘}’]
[]

[’{u’, ‘deapth’, ‘: u’, ‘-0.01’, ‘, u’, ‘acly’, ‘: 0, u’, ‘yaw’, ‘: u’, ‘343.81’, ‘, u’, ‘aclx’, ‘: 0, u’, ‘pitch’, ‘: u’, ‘5.13’, ‘, u’, ‘thrust’, ‘: u’, ‘0.00’, ‘, u’, ‘hdgd’, ‘: u’, ‘343.81’, ‘, u’, ‘aclz’, ‘: 0, u’, ‘roll’, ‘: u’, ‘0.00’, ‘}’]
[]

[’{u’, ‘deapth’, ‘: u’, ‘-0.01’, ‘, u’, ‘acly’, ‘: 0, u’, ‘yaw’, ‘: u’, ‘343.81’, ‘, u’, ‘aclx’, ‘: 0, u’, ‘pitch’, ‘: u’, ‘5.13’, ‘, u’, ‘thrust’, ‘: u’, ‘0.00’, ‘, u’, ‘hdgd’, ‘: u’, ‘343.81’, ‘, u’, ‘aclz’, ‘: 0, u’, ‘roll’, ‘: u’, ‘0.00’, ‘}’]
[]

Can somebody tell me why?


#10

Hi Nasir,

It is because you are presumably using the BNO055 IMU sensor (the IMU2) as opposed to the MPU9150. In this case, the modifications to CMPU9150.cpp aren’t getting called, because it is the BNO055 that is loaded up and updating. I am working on an update that abstracts away most of the differences of the individual IMUs so that you don’t have to worry about that anymore and will also make the raw IMU outputs available to you (accel, gyro, and mag). Just a bit behind because of the holidays and travel. I’ll post a link to the new firmware branch with these updates early this week.

Charles


#11

Thanks Charles, I figured that out yesterday silly mistake :frowning:

Really looking forward to the updates.

In the mean time would you have any pointers on how accurately the dead reckoning with bon055 is?


#12

Well, you can lookup the data sheet on the bno055 to see the tolerances on the gyros and accelerometer to get an idea. General belief is that integrating the accelerometer data to determine velocity causes the error to grow much to large to be of much use for dead reckoning. That said, I’m not aware of anyone having tested it yet.


#13

@Nasir_Ahsan and @opegieka

https://github.com/OpenROV/openrov-software-arduino/tree/feature/rawimuoutput-bno

This feature branch is a quick add-in of raw value output support for the BNO055. It isn’t the full abstraction yet, but this should give you the data you need. The only changes are in AConfig.h and CBNO055.cpp. To turn on/off raw outputs for each specific type of output, change the define value for it in AConfig.h to 1 or 0 respectively. Defaultly, I’ve turned the raw accelerometer data on for you, which will be output at 100hz in the format “RACC: x | y | z;”. You can split that up across multiple messages, if you don’t feel like bothering with parsing it on the other end, it just saves a little overhead.

The raw outputs that you have access to are:

  • Accelerometer “RACC” (100hz)
  • Linear Acceleration “RLACC” (100hz, fusion mode only)
  • Gyro “RGYR” (100hz)
  • Magnetometer “RMAG” (20hz, fusion mode only)

One thing that might cause some problems for you is the IMU mode switching that currently happens (bouncing back and forth between IMU mode and Fusion Mode). I’ve renamed the inOverride variable to modeSwitchingEnabled in CBNO055.cpp which defaults to true. When the IMu is in IMU mode, you won’t have access to the Mag or Linear Accel data, so you can initialize that value to false if you would like it to always stay in Fusion mode so that you get uninterrupted data (although beware that it could be poorly calibrated data depending on the state of the fusion algorithm).

Haven’t tested this yet, but I was inspired this last weekend to do some work with accelerometer integration and Kalman filtering, so I’ll be playing with it more myself this week. Let me know if you run into any problems or have any questions.


#14

Thanks Charles!! I’ll try it out.


#15

Hi @charlesdc,

I had a go at integrating the new files.

So I tried this three ways:

  1. Just replace the CBO055.cpp and AConfig.h files with the analogues in your package.
  2. Replace the whole parent directory of OpenROV within which these files reside
  3. Replace all the contents of the arduino folder at /opt/openrov/ with the contents of the repo above.

after each change i recompile using:

  1. sudo /opt/openrov/cockpit/linux/arduino/firmware-installfromsource.sh
  2. sudo /etc/init.d/openrov restart

This is the output I consistently get when recompiling each time

rov@openrov:~$ ./redoArduino.sh
unpacking
staging: build dir is /tmp/tmp.ICcLayQHA5
staged src in to build folder

staging plugins
unpacked into folder /tmp/tmp.ICcLayQHA5
compilling in /tmp/tmp.ICcLayQHA5
mv: cannot stat ‘/tmp/tmp.ICcLayQHA5/src/Device.cpp’: No such file or directory
sed: can’t read /tmp/tmp.ICcLayQHA5/src/Device.cpp.template: No such file or directory
rm: cannot remove ‘/tmp/tmp.ICcLayQHA5/src/Device.cpp.template’: No such file or directory
Searching for Board description file (boards.txt) … /usr/share/arduino/hardware/arduino/boards.txt
Searching for Arduino lib version file (version.txt) … /usr/share/arduino/lib/version.txt
Detecting Arduino software version … 1.0.5 (1.0.5)
Searching for Arduino core library … /usr/share/arduino/hardware/arduino/cores/arduino
Searching for Arduino variants directory … /usr/share/arduino/hardware/arduino/variants
Searching for Arduino standard libraries … /usr/share/arduino/libraries
Searching for make … /usr/share/arduino/hardware/tools/avr/bin/make
Searching for avr-gcc … /usr/share/arduino/hardware/tools/avr/bin/avr-gcc
Searching for avr-g++ … /usr/share/arduino/hardware/tools/avr/bin/avr-g++
Searching for avr-ar … /usr/share/arduino/hardware/tools/avr/bin/avr-ar
Searching for avr-objcopy … /usr/share/arduino/hardware/tools/avr/bin/avr-objcopy
src/OpenROV.ino
Searching for Arduino lib version file (version.txt) … /usr/share/arduino/lib/version.txt
Detecting Arduino software version … 1.0.5 (1.0.5)
Scanning dependencies of src
Scanning dependencies of arduino
Scanning dependencies of EEPROM
Scanning dependencies of SPI
src/CModule.cpp
src/LibMPU_Calibration.cpp
src/CI2C.cpp
src/CMotor.cpp
src/NArduinoManager.cpp
src/NConfigManager.cpp
src/CMS5803_XXBA.cpp
src/Device.cpp
src/CMS5803_XXBALib.cpp
src/NModuleManager.cpp
src/CCameraMount.cpp
src/LibMPU_I2CDevice.cpp
src/CBNO055.cpp
src/CBNO055.cpp: In member function ‘virtual void CBNO055::Update(CCommand&)’:
src/CBNO055.cpp:91:5: error: ‘inOverride’ was not declared in this scope
inOverride = false;
^
src/CBNO055.cpp:100:5: error: ‘inOverride’ was not declared in this scope
inOverride = true;
^
src/CBNO055.cpp:107:5: error: ‘inOverride’ was not declared in this scope
inOverride = true;
^
.build/mega2560/Makefile:284: recipe for target ‘.build/mega2560/src/CBNO055.o’ failed
make: *** [.build/mega2560/src/CBNO055.o] Error 1
Make failed with code 2
firmware-build.sh: : Compile of the Arduino image failed.
firmware-installfromsource.sh: Building Firmware Failed! Aborting
[ ok ] Restarting OpenROV NodeJS server: openrov[…] —> OpenROV NodeJS server stopped: openrov[…] —> started OpenROV NodeJS server: openrov.
rov@openrov:~$


#16

Hey @charlesdc,

So I declared the inOverride boolean variable in the namespace and that did the trick!

I can see the RACC xyz and RLACC xyz. The RLACC seem to be zero at no motion (and change upon motion) however RACC seems to have values like 0.29, 0.9, 9.69 at no motion, i.e., openROV is still. Is this because no fusion has been done and no biases have been offset?

Many thanks it works great. Now to the EKF stuff!


#17

Ah, sorry about that. I forgot to replace a few of the inOverride’s with “modeSwitchingEnabled” (I renamed the variable in an earlier commit). I just updated the code on github, so either replace those “inOverride’s” yourself or grab the latest code.

As for the outputs, I believe the RACC values are going to be uncalibrated, as you guessed. The RLACC values are basically the RACC with gravity and offsets taken into account (and maybe some other magic under the hood, the BNO055 is a black box so nobody really knows what math it does in its fusion).


#18

Hey Charles,

Thanks for that. I’ll grab the code but it worked with only one declaration.

I see makes sense why tge RACC is off. I’Ll start playing around with the Imu data but I highly doubt that it would be ok after a few seconds of position tracking even though the rlacc values have been fused with mag data.


#19

What units is LACC measured in?


#20

Right now it is in mg. You can change it to m/s^2 by modifying the following code in CAdaBNO055.cpp. Change the 1 to a 0 in the last line:

bool CAdaBNO055::SetUpUnitsAndOrientation()
{
    // Select the output units
    uint8_t unitSelect =    ( 0 << 7 ) |    // Orientation = Android
                            ( 0 << 4 ) |    // Temperature = Celsius
                            ( 0 << 2 ) |    // Euler = Degrees
                            ( 0 << 1 ) |    // Gyro = Deg/s
     /*Set this to 0*/      ( 1 << 0 );     // Accelerometer = mg

See page 30 of the datasheet for other unit selection settings: