Velocity and distance from Prop shield plus WAV playing issues

Status
Not open for further replies.

snowskijunky

Active member
So my goal here is to replace the electronics of a model train. I'm using a teensy 3.6+prop shield so that later on I can hook up a motor controller and RFID reader that will make use of the teensy 3.6's extra I2c and/or SPI ports (plus it already has an on board sd card).

I'd like to be able to calculate velocity and distance traveled so that I can play a chuffing sound every "x amount" distance traveled. That amount will be determined by the circumference of the locomotive's wheels so that I can play 4 chuffs per revolution of the driving wheel. I am wondering if it is possible to calculate velocity and distance from the accelerometer and what the best way to go about doing it would be, if there are any examples, etc. The calibration seems to have worked well (it came out rather spherical) but the acceleration values I'm reading are jumpy, and the heading reading from the orientation is constantly changing despite the device being still.
Code:
// Full orientation sensing using NXP's advanced sensor fusion algorithm.
//
// You *must* perform a magnetic calibration before this code will work.
//
// To view this data, use the Arduino Serial Monitor to watch the
// scrolling angles, or run the OrientationVisualiser example in Processing.

#include <NXPMotionSense.h>
#include <Wire.h>
#include <EEPROM.h>

NXPMotionSense imu;
NXPSensorFusion filter;

void setup() {
  Serial.begin(9600);
  imu.begin();
  filter.begin(100);
}

void loop() {
  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;
  float roll, pitch, heading;

  if (imu.available()) {
    // Read the motion sensors
    imu.readMotionSensor(ax, ay, az, gx, gy, gz, mx, my, mz);

    // Update the SensorFusion filter
    filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);

    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    
    Serial.print("Orientation: ");
    Serial.print(ax);
    Serial.print(" ");
    Serial.print(ay);
    Serial.print(" ");
    Serial.println(az);
  }
}



Any help or guidance here would be much appreciated. Thanks in advance!
 
Last edited:
Ok, so I've managed to get somewhere on my own. I wrote this for velocity, and it seems to work on the serial plotter! The only problem is damn gravity... if the board isn't held perfectly flat, the velocity reading goes nuts due to gravitational acceleration. Also, it might help to know what units the accelerometer is reading in.

EDIT: UPDATED CODE TO INCLUDE DISTANCE CALC
Code:
// To view this data, use the Arduino Serial Monitor to watch the
// scrolling angles, or run the OrientationVisualiser example in Processing.

#include <NXPMotionSense.h>
#include <Wire.h>
#include <EEPROM.h>

NXPMotionSense imu;
NXPSensorFusion filter;

float accel1;
float accel2;
float velocity = 0;
float preVelocity = 0;
float distanceCur = 0;
float distancePrev = 0;
float distance = 0;
float[] gravityComp = new float[3];

void setup() {
  Serial.begin(9600);
  imu.begin();
  filter.begin(100);
  pinMode(13, OUTPUT);  
}

void loop() {
  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;
  float roll, pitch, heading;

  if (imu.available()) {
    // Read the motion sensors
    //imu.readMotionSensor(ax, ay, az, gx, gy, gz, mx, my, mz);

    // Update the SensorFusion filter
    //filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);

    // print the heading, pitch and roll
    //roll = filter.getRoll();
    //pitch = filter.getPitch();
    //heading = filter.getYaw();
    
    imu.readMotionSensor(ax, ay, az, gx, gy, gz, mx, my, mz);
    accel1 = ax;
    delay(5);
    imu.readMotionSensor(ax, ay, az, gx, gy, gz, mx, my, mz);
    accel2 = ax;
    velocity = preVelocity + (((accel1+accel2)/2)*(.005));
    distanceCur = ((velocity + preVelocity)/2)*0.1;
    distancePrev = distance;
    distance = abs(distanceCur)+abs(distancePrev);
    preVelocity=velocity;
    
    Serial.print("Velocity: ");
    Serial.print(velocity);
    Serial.print("Distance: ");
    Serial.println(distance);
    //Serial.print(" ");
    //Serial.println(az);
  }
}
 
Last edited:
You may want to check this application note out from NXP: http://www.nxp.com/docs/en/application-note/AN3397.pdf

Also, I remember reading some where that you need to remove gravity and use dynamic acceleration????? http://www.varesano.net/blog/fabio/simple-gravity-compensation-9-dom-imus

Code:
void gravityCompensateDynAcc() {
  float[] g = new float[3];
  
  // get expected direction of gravity in the sensor frame
  g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);
  g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);
  g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
  
  // compensate accelerometer readings with the expected direction of gravity
  dyn_acc[0] = acc[0] - g[0];
  dyn_acc[1] = acc[1] - g[1];
  dyn_acc[2] = acc[2] - g[2];
}
 
Last edited:
Thank you mjs513, dynamic acceleration is definitely what I'm looking for, I'll check out that second link you posted - for some reason it wasn't working at first. I've managed to add distance, and everything seems to be looking much better now, though I'm still not sure how exactly to write the code to compensate for gravity.
 
You need to get the quaternions out from the Mahoney filter (have to uncomment that). Unfortunately don't think it allows you right now to get them using the nxpmotionsense library. Think there is a issue out there to incorporate the change.

But if you get it out you just create a global dyn_acc[3]. Call the function in my previous post and then use dyn_acc[0] in place of ax, etc in your calculations.
 
How exactly would I go about getting the quaternions out from the Mahoney filter? Would my q[0], q[1], and q[2] just be my roll, pitch, and yaw from the example program? What about q[3]?
 
Unfortunately the answer is no. The Madgwick and Mahony filters generate quaternions and then convert them to Euler angles (Yaw, Pitch and Roll). I am trying to find the way to reverse that for you. Either way I think I incorporated the changes somewhere. I will look for it and post when I find it - tomorrow probably.
 
Ok, I gotcha. I've been looking into it all night and am not having much luck, so any info you can find I would greatly appreciate it. This is not turning out to be as trivial as I was hoping it would be haha... but thank you for all your help so far! I managed to get distance measurements working, so now just to figure out the audio and quaternions.

EDIT: I found this: http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/
These equations might be what I'm looking for. I'll try it out tomorrow some time.


c1 = cos(heading / 2)
c2 = cos(attitude / 2)
c3 = cos(bank / 2)
s1 = sin(heading / 2)
s2 = sin(attitude / 2)
s3 = sin(bank / 2)

w = c1 c2 c3 - s1 s2 s3
x = s1 s2 c3 +c1 c2 s3
y = s1 c2 c3 + c1 s2 s3
z = c1 s2 c3 - s1 c2 s3
 
Thank you mjs513, dynamic acceleration is definitely what I'm looking for, I'll check out that second link you posted - for some reason it wasn't working at first.

I'm not sure you will get far with an IMU.
Of course you could remove gravity and will get dynamic acceleration. Now think about what type of acceleration you will have. only when you change speed you will get acceleration, otherwise you have zero. if your speed integration is not perfect, then distance integration will diverge from reality pretty soon.
However your speed estimation is typically based on short accelerations (the force by the motor is typically too strong for the low-weight train), so your integration will only be very rough. Also any roughness in the track will result in acceleration hits (spikes or noise) that should be removed before integration.
So, good speed estimation is a little bit tricky, but of course you can/should try.

can you not put a small (optical?) rotation sensor onto the wheel axes and read these?
 
@snowskijunky. WMXZ is correct that it will be though to get good distance measurements with an IMU, not only because of the double integration you have to perform to get distance but because of noise in the accelerometer measurements (normal for mems sensors). But some people have had success depending on the application I guess. You should give it a try - it can't hurt.

As for putting an optical sensor on the wheels, think that is going to be dependent on the gauge. Didn't see that in your original post.

Anyway, think you need to take a look at the prop shield web page to understand how to do a sensor calibration.

Second, I am attaching the modified NXPMotionSense.h file. Just replace the current one with this one in the library folder. Your program would then look something like this (caveat - I haven't tested this:
Code:
#include <Wire.h>
#include <EEPROM.h>

NXPMotionSense imu;
Mahony filter;

float accel1;
float accel2;
float velocity = 0;
float preVelocity = 0;
float distanceCur = 0;
float distancePrev = 0;
float distance = 0;
float dyn_acc[3];

void setup() {
  Serial.begin(9600);
  imu.begin();
  filter.begin(100);
  pinMode(13, OUTPUT);  
}

void loop() {
  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;
  float roll, pitch, heading;

  if (imu.available()) {
    
    imu.readMotionSensor(ax, ay, az, gx, gy, gz, mx, my, mz);
	filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
	NXPSensorFusion::Quaternion_t quaternion = filter.getQuaternion
	gravityCompensateDynAcc();
    //accel1 = ax;
	accel1 = dyn_acc[0];
    delay(5);
	
    imu.readMotionSensor(ax, ay, az, gx, gy, gz, mx, my, mz);
	filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
	NXPSensorFusion::Quaternion_t quaternion = filter.getQuaternion
    //accel2 = ax;
	accel2 = dyn_acc[0];
	
    velocity = preVelocity + (((accel1+accel2)/2)*(.005));
    distanceCur = ((velocity + preVelocity)/2)*0.1;
    distancePrev = distance;
    distance = abs(distanceCur)+abs(distancePrev);
    preVelocity=velocity;
    
    Serial.print("Velocity: ");
    Serial.print(velocity);
    Serial.print("Distance: ");
    Serial.println(distance);
    //Serial.print(" ");
    //Serial.println(az);
  }
}

void gravityCompensateDynAcc() {
  float g[3];
  
  // get expected direction of gravity in the sensor frame
  g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);
  g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);
  g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
  
  // compensate accelerometer readings with the expected direction of gravity
  dyn_acc[0] = acc[0] - g[0];
  dyn_acc[1] = acc[1] - g[1];
  dyn_acc[2] = acc[2] - g[2];
}

You will also need to get the Mahony library: https://github.com/PaulStoffregen/MahonyAHRS.

wmxz is also correct that if you have constant acceleration you will have to make some further adjustments. For instance if you know the train is moving the if accel2-accel1 = 0 then just use the previous velocity, etc.

Let's us know how it goes.
 

Attachments

  • NXPMotionSense.h
    7.5 KB · Views: 105
I do apologize for not providing some more info, but it is O gauge. Most manufacturers actually use a flywheel with black/white strips and an optical line sensor like this: https://www.sparkfun.com/products/9453 to get distance, since they know the exact distance traveled for each rotation of the flywheel. I was originally shying away from this since in the future I would have to add flywheels and optical sensors to several of my locomotives, but I'm starting to think that might be the better option in the long run. Regardless, I will give your code a shot mjs and I will report back on how it goes... might not be til this evening though now. Thank you guys VERY MUCH!!

UPDATE: Ok, so I actually got it working. It is definitely compensating a little bit, but there is a LOT of drift. I'll see if I can make it more accurate. As I mentioned in the first post, my calibration looked pretty good, almost exactly like the on ein the tutorial...but I may try that again just in case. Also, I noticed the teensy has been getting quite warm, is that normal?

UPDATE 2: I've not had much luck smoothing the acceleration compensation out. No matter what I seem to do, there is always some drift which just compounds and compounds. If the locomotive were running for more than 5 minutes, the chuffing would end up totally wack. For now, I think I'm going to scrap the idea and maybe come back to it later. Having a physical sensor seems to be the better approach, but thank you all for helping me give it a shot.
 
Last edited:
I tried yours method, but I take a error which is "invalid use of non-static member function" for "NXPSensorFusion::Quaternion_t quaternion = filter.getQuaternion" command line. Could you help me for this issue ?
 
Can you post your code or what sketch you are running, what Teensy you are using etc? What library are you using etc?
 
Can you post your code or what sketch you are running, what Teensy you are using etc? What library are you using etc?

I installed https://github.com/PaulStoffregen/MahonyAHRS and modified NXPMotionSense.h. After I am tried to compiled the code whicih is at the below. Other details are I am using Teensy 3.6, prop shield and arduino 1.8.5 and Teenstduino compiler.


Code:
#include <NXPMotionSense.h>
#include <MahonyAHRS.h>
#include <Wire.h>
#include <EEPROM.h>

NXPMotionSense imu;
Mahony filter;

float accel1;
float accel2;
float velocity = 0;
float preVelocity = 0;
float distanceCur = 0;
float distancePrev = 0;
float distance = 0;
float dyn_acc[3];

void setup() {
  Serial.begin(9600);
  imu.begin();
  filter.begin(100);
  pinMode(13, OUTPUT);  
}

void loop() {
  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;
  float roll, pitch, heading;

  if (imu.available()) {
    
    imu.readMotionSensor(ax, ay, az, gx, gy, gz, mx, my, mz);
  filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
  NXPSensorFusion::Quaternion_t quaternion = filter.getQuaternion
  gravityCompensateDynAcc();
    //accel1 = ax;
  accel1 = dyn_acc[0];
    delay(5);
  
    imu.readMotionSensor(ax, ay, az, gx, gy, gz, mx, my, mz);
  filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
  NXPSensorFusion::Quaternion_t quaternion = filter.getQuaternion
    //accel2 = ax;
  accel2 = dyn_acc[0];
  
    velocity = preVelocity + (((accel1+accel2)/2)*(.005));
    distanceCur = ((velocity + preVelocity)/2)*0.1;
    distancePrev = distance;
    distance = abs(distanceCur)+abs(distancePrev);
    preVelocity=velocity;
    
    Serial.print("Velocity: ");
    Serial.print(velocity);
    Serial.print("Distance: ");
    Serial.println(distance);
    //Serial.print(" ");
    //Serial.println(az);
  }
}

void gravityCompensateDynAcc() {
  float g[3];
  
  // get expected direction of gravity in the sensor frame
  g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);
  g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);
  g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
  
  // compensate accelerometer readings with the expected direction of gravity
  dyn_acc[0] = acc[0] - g[0];
  dyn_acc[1] = acc[1] - g[1];
  dyn_acc[2] = acc[2] - g[2];
}


Error mesages:

Code:
Derleme seçenekleri değiştirildi, tümü yeniden derleniyor.
imu3: In function 'void loop()':
imu3:35: error: 'class Mahony' has no member named 'getQuaternion'
   NXPSensorFusion::Quaternion_t quaternion = filter.getQuaternion

                                                     ^

imu3:43: error: redeclaration of 'NXPSensorFusion::Quaternion_t quaternion'
   NXPSensorFusion::Quaternion_t quaternion = filter.getQuaternion

                                 ^

C:\Users\Mustafa Can\Desktop\imu3\imu3.ino:35:33: note: 'NXPSensorFusion::Quaternion_t quaternion' previously declared here

   NXPSensorFusion::Quaternion_t quaternion = filter.getQuaternion

                                 ^

imu3:43: error: 'class Mahony' has no member named 'getQuaternion'
   NXPSensorFusion::Quaternion_t quaternion = filter.getQuaternion

                                                     ^

imu3:35: warning: unused variable 'quaternion' 
   NXPSensorFusion::Quaternion_t quaternion = filter.getQuaternion

                                 ^

imu3:29: warning: unused variable 'roll' 
   float roll, pitch, heading;

         ^

imu3:29: warning: unused variable 'pitch' 
   float roll, pitch, heading;

               ^

imu3:29: warning: unused variable 'heading' 
   float roll, pitch, heading;

                      ^

imu3: In function 'void gravityCompensateDynAcc()':
imu3:66: error: 'q' was not declared in this scope
   g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);

               ^

imu3:71: error: 'acc' was not declared in this scope
   dyn_acc[0] = acc[0] - g[0];

                ^

'class Mahony' has no member named 'getQuaternion'
 
There are a couple of things going on. The first and foremost there is no function called getQuaternion in NXPSensorFunsion so that is why you are getting the associated error. To fix that I modified the MahoneyAHRS.h file to get the quaternions out, see attach ( View attachment MahonyAHRS.zip ). Just replace the existing one with the attached version after you unzip it of course. To access the quaternions use the following :
Code:
  q[0] = filter.q0;
  q[1] = filter.q1;
  q[2] = filter.q2;
  q[3] = filter.q3;

You can just replace the NXPSensoFusion::Quaternion_t Quaternion = filter.getQuaternion; line in your sketch with the lines above and you can that problem fixed. You have a couple of other bugs that I think you can handle :). You may want to check this application note out from NXP: http://www.nxp.com/docs/en/application-note/AN3397.pdf as well.

Remember you need to do a calibration on your sensors as well. See https://www.pjrc.com/store/prop_shield.html. I did not check the code for converting acceleration to distance - check the reference I gave you. Think you have to some tweaking to that code segment as well.
 
Status
Not open for further replies.
Back
Top