uNav INS

Got it changed, but still can only run at GPS_BAUD 115200. Will take a look at settings in u-Connector, but maybe that's all my particular uBlox can run at.
 
Don

That's strange. I setup the M8n in u-center for baud etc first then attached to Teensy.

Oh, BTW, I got the 5637 sensor working and sent the altitude along with gps altitude to Tviewer. The altitude based on press/temp was 11meters give or take a little bit, but the GPS altitude was about 40 meters and varied about 5 meters only ran it for about 15 minutes. My actual is about 19 meters.

Mike
 
My mistake, didn't have the baud rate on the M8N side set to 460800. Duh...

I'm not sure what my actual altitude is here. My lot survey says we're only 22 feet above sea level, so guessing that's 22' MSL or about 6.7m MSL. I need to fire up my drone and see what the PixHawk w/ RTK says...

I should have my baro board by Sat...
Don
 
floats vs doubles, is it better to get the mean, avg, deviation better by 64bit double or 32bit float precision?

I say this because I don't know anyone who tested doubles yet in circular buffer, even though it compiles
 
Mike,
By the way, the Marmot board I got from Brian has a BME280 temp/baro/humidity sensor built in on it.

tonton81 - good question. On my list f things to do is to convert the EKF code (uNavINS.cpp) to all Doubles to see how it does. I think Brian might be doing this as well.

Don
 
mjs513 said:
Hi Tim. Instead of using the dt with the pause delay included did you try setting it up so at start of pause it saves the lastDt = dt and end of pause reset dt to lastDt seen?
Click to expand...

Correcting or catching oversized dt is possible - meant to post that before heading to bed. If that long many samples missed probably better to skip and start fresh as something went wrong?
 
I did a quick test using:

Code: 
  Circular_Buffer<XXX, 16> myFloats;
  myFloats.push_back(3.14159);
  myFloats.push_back(12.3456);
  myFloats.push_back(78.91234);
  myFloats.push_back(7.91234);
  myFloats.push_back(11.91234);
  myFloats.push_back(58.91234);
  myFloats.push_back(18.91234);
  //  myFloats.push_back(2.91234);

  Serial.print("SUM: "); Serial.println(myFloats.sum(), 5);
  Serial.print("MIN: "); Serial.println(myFloats.min(), 5);
  Serial.print("MAX: "); Serial.println(myFloats.max(), 5);
  Serial.print("MEDIAN: "); Serial.println(myFloats.median(1), 5);

  uint8_t blah[5];
  //  qsort(blah, 5, sizeof(uint8_t), *(uint8_t*)a - *(uint8_t*)b);
  Serial.print("AVG: "); Serial.println(myFloats.average(), 5);
  uint8_t _size = myFloats.size();
  Serial.print("Deviation: "); Serial.println(myFloats.deviation(), 8);
  for ( uint8_t i = 0; i < _size; i++ ) {
    Serial.print(myFloats.pop_front(), 5); Serial.print(" ");
  } Serial.println();

while(1);

this is the result:

with Circular_Buffer<floats, 16> myFloats; :
Code: 
SUM: [COLOR="#FF0000"]192.04887[/COLOR]
MIN: 3.14159
MAX: 78.91234
MEDIAN: 12.34560
AVG: 27.43556
Deviation: [COLOR="#FF0000"]27.13280106[/COLOR]
3.14159 7.91234 11.91234 12.34560 18.91234 58.91234 78.91234

with Circular_Buffer<double, 16> myFloats; :
Code: 
SUM: [COLOR="#FF0000"]192.04889[/COLOR]
MIN: 3.14159
MAX: 78.91234
MEDIAN: 12.34560
AVG: 27.43556
Deviation: [COLOR="#FF0000"]27.13280006[/COLOR]
3.14159 7.91234 11.91234 12.34560 18.91234 58.91234 78.91234

I guess doubles is working, but I also guess it needs a bigger float to matter, as only the SUM and Deviation looks slightly off

Note: All returns are in Type format, so if your circular_buffer is a float, a float is returned, double, double is returned, and if its an int, int is returned (truncated, of course)
 
Mike,
Been thinking about your earlier post, about changing up the Update portion to execute for any of the following scenarios:
1) Both GPS and mag measurements are available (what we're running now)
2) GPS only meas are avail (we ran before mag was added)
3) Mag only meas are avail (i.e., no GPS)

Right now we use an interrupt from the IMU to set the newIMUData flag when there's new IMU data. I need to study the MPU9250 datasheet, but I'm guessing this may mean ANY new IMU data? Or does it mean new accel and gyro data?

My reason for asking is how will we know when we have valid NEW mag data, since it's being provided at a slower rate than the accel / gyro new data?

Wonder if we can set up a newMagData flag?

Back to the datasheet, see what I can find...

Don
 
It looks like, from the MPU9250 datasheet, that INT goes high if there's new data in the buffer.

Looking at Brian's MPU9250 library in github, he says

For accel and gyro:
Output Data Rate = 1000 / (1+SRD)

For mag:
if 0 >= SRD <= 9, mag output rate = 100Hz
if SRD > 9, mag output rate = 8Hz

So seems like we might set SRD to low number, like 0 or 1, to get fast accel and gyros. Then we know that as long as we wait 10ms, we should have fresh mag data.

Don
 
probably better to skip and start fresh as something went wrong?
Click to expand...
Tim - might be interesting to try, you took it to another level when I was only thinking about pause. Think you would have to becareful with dt though. Right now this is the only thing running on the Teensy, but if we start adding other code for other functions besides a AHRS/INS system dt may get screwy especially if you are talking about high data rates. You may be talking about just dedicating a single T3.5 for this function and another for other functions.

Don - didn't think the mag had a separate interrupt. You can always check out the FIFO option :)
 
Yep, looking like no INT for magnetometer.

I'll keep thinking on it... Still seems like it would be sweet to use PPS to start the process for each void loop. Oh well.

I guess the mag reading, since it's running at 100Hz, really can be up to 10ms old when we read it anyway. So if a drone is yawing really fast at 500 deg/s, the yaw reading could be up to 5 deg off (old) it seems. Hmmm...
 
I was wondering when lower freq MAG data was read ... but had not looked

mjs513 said:
Tim - might be interesting to try, you took it to another level when I was only thinking about pause. Think you would have to becareful with dt though. Right now this is the only thing running on the Teensy, but if we start adding other code for other functions besides a AHRS/INS system dt may get screwy especially if you are talking about high data rates. You may be talking about just dedicating a single T3.5 for this function and another for other functions.

Don - didn't think the mag had a separate interrupt. You can always check out the FIFO option :)
Click to expand...

Easiest compare to do would be a counter on IMU data int's. Record time and do IMU_iCnt++. Back in loop():: if (1+IMU_iCnt_last != IMU_iCnt ) { Serial.print( "data lost"); // skip update & propagate }
>> But any discard of IMU data is a hole in the system - ... I put in 5 liters - how come only 4 liters came out? ... I went up 5 meters and down 4 meters and hit the ground.
>> So this seems a place to note an error.

That IMU_iCnt could lead to a way to track the need for MAG reads too - given the ratio of MAG to IMU_SRD {MAG_rat=5 for SRD==1} :: if ( !( IMU_iCnt%MAG_rat) ) { // read/use new MAG data }
 
Don - Tim
There isn't a Data ready pin for the Mag but you can read a register to get when the data is ready - that is probably the way Brian is doing it in the library.

I was looking at the AK8963 mag datasheet and the Time for measurement is 7.2 ms with a max of 9ms. But that is for single measurement mode. I didn't see what it is for continuous measurement mode. Nope found it - max mag reading 100hz in continuous mode
 
It looks like this gets all the device values each time in a single read:

/* reads the most current data from MPU9250 and stores in buffer */
int MPU9250::readSensor() {


It gets 10 values from 20 bytes.

something called tcounts {temp reading?} and then X,Y,Z for a, g, and h - that seem to be Accel, Gyro and Mag
 
Did a quick test on MAG changes - it is returned each time and updated at 95 Hz { this is with my unit sitting stationary }.

-----Loop#:487220 IMU#:500 >> 1Sec:99924712 >> IMUcc:137661 >> Mcnt:95 & !Mcnt:405

This shows that out of 500 IMU readings in one Second there is a change in the X,Y,Z returned for MAG 95 times and No change in MAG data 405 times.

The data is already returned each time - Not seeing any MAG data change notice - so I did this data change detection::
Code: 
float This_Mag[4];
      This_Mag[0] = Imu.getMagX_uT();
      This_Mag[1] = Imu.getMagY_uT();
      This_Mag[2] = Imu.getMagZ_uT();
      This_Mag[3] = This_Mag[0] + This_Mag[1] + This_Mag[2];
      if ( This_Mag[3] != Last_Mag ) Mcnt++;
      else McntN++;
      Last_Mag = This_Mag[3];

      Filter.update(uBloxData.iTOW, uBloxData.velN, uBloxData.velE, uBloxData.velD,
                    uBloxData.lat * D2R, uBloxData.lon * D2R, uBloxData.hMSL,
                    Imu.getGyroX_rads(), Imu.getGyroY_rads(), Imu.getGyroZ_rads(),
                    Imu.getAccelX_mss(), Imu.getAccelY_mss(), Imu.getAccelZ_mss(),
                    This_Mag[0], This_Mag[1], This_Mag[2]);

<edit>::
The count is not always 95 - sometimes 89. Even wiggling it.

With a change to the MPU9250 lib - the data could be sent back in a STRUCT rather than 9 calls() for the data - and the MAG change could be indicated on return - and similar check with the three uint16_t's rather than on return with three FLOATS.
 
I have a change for MPU9250 - saves a few cycles and provides MagD=> i.e. MagDelta true or false.

This:: -----Loop#:595207 IMU#:502 >> 1Sec:100272072 >> IMUcc:127208 >> Mcnt:93 & !Mcnt:409
.vs. :: -----Loop#:487220 IMU#:500 >> 1Sec:99924712 >> IMUcc:137661 >> Mcnt:95 & !Mcnt:405

The RAW MAG 16 bit counts do trickle I found - so I could not save doing the float _magScale math as I first tried :(

I modeled after readSensorCounts that takes POINTERS, this saves function calls to get the class data 9 times over per IMU read! - seems this is where the 100K more Loop# savings comes from.

Then in this code that is a duplication of ReadSensors and replaces it with readSensorPMD { i.e. >> Pointers MAG Diff }:
Code: 
// added to HEADER ::     \libraries\MPU9250\MPU9250.h
void     int readSensorPMD(float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* hx, float* hy, float* hz, bool* MagD);

// Added to \libraries\MPU9250\MPU9250.cpp
int MPU9250::readSensorPMD(float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* hx, float* hy, float* hz, bool* MagD)
{
  _useSPIHS = true; // use the high speed SPI for data readout
  // grab the data from the MPU9250
  if (readRegisters(ACCEL_OUT, 21, _buffer) < 0) {
    return -1;
  }
  // combine into 16 bit values
  _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1];
  _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3];
  _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5];
  _tcounts = (((int16_t)_buffer[6]) << 8) | _buffer[7];
  _gxcounts = (((int16_t)_buffer[8]) << 8) | _buffer[9];
  _gycounts = (((int16_t)_buffer[10]) << 8) | _buffer[11];
  _gzcounts = (((int16_t)_buffer[12]) << 8) | _buffer[13];
  _hxcounts = (((int16_t)_buffer[15]) << 8) | _buffer[14];
  _hycounts = (((int16_t)_buffer[17]) << 8) | _buffer[16];
  _hzcounts = (((int16_t)_buffer[19]) << 8) | _buffer[18];

  // transform and convert to float values
  *ax = _ax = (((float)(tX[0] * _axcounts + tX[1] * _aycounts + tX[2] * _azcounts) * _accelScale) - _axb) * _axs;
  *ay = _ay = (((float)(tY[0] * _axcounts + tY[1] * _aycounts + tY[2] * _azcounts) * _accelScale) - _ayb) * _ays;
  *az = _az = (((float)(tZ[0] * _axcounts + tZ[1] * _aycounts + tZ[2] * _azcounts) * _accelScale) - _azb) * _azs;
  *gx = _gx = ((float)(tX[0] * _gxcounts + tX[1] * _gycounts + tX[2] * _gzcounts) * _gyroScale) - _gxb;
  *gy = _gy = ((float)(tY[0] * _gxcounts + tY[1] * _gycounts + tY[2] * _gzcounts) * _gyroScale) - _gyb;
  *gz = _gz = ((float)(tZ[0] * _gxcounts + tZ[1] * _gycounts + tZ[2] * _gzcounts) * _gyroScale) - _gzb;
  [B]*hx[/B] =  (((float)(_hxcounts) * _magScaleX) - _hxb) * _hxs;
  [B]*hy[/B] =  (((float)(_hycounts) * _magScaleY) - _hyb) * _hys;
  [B]*hz[/B] =  (((float)(_hzcounts) * _magScaleZ) - _hzb) * _hzs;

  
  if ( [COLOR="#FF0000"](*hx == _hx) && (*hy == _hy) && (*hz == _hz)[/COLOR] ) {
    *MagD = false;
  }
  else {
    *MagD = true;
[B]    _hx = *hx;
    _hy = *hy;
    _hz = *hz;
[/B]  }
  _t = ((((float) _tcounts) - _tempOffset) / _tempScale) + _tempOffset;
  return 1;
}

No added variables - I just set the return *h? values with the NEW data - then compare the class _h? OLD data to those, and update as indicated.

The small trickle of MAG _h? counts falls out in the Scale and conversion to float and matches the results I got testing in the sketch before moving the code to the library!

This should result in NO CHANGE to observed/returned data from the IMU - but yields a Boolean flag to indicate return data showing change in MAG data!

Brian:: Is this a worthy change/addition for the MPU9250 library?

Is there any USE or VALUE in returning the measure TEMP from the IMU? It is there and might have value - like say when Don is out in the sun and getting odd results?

ALL:: Note - like readSensor() the call to readSensorPMD() returns 1 or -1 indicating if valid data was received from the IMU. If not then the data will be unchanged from the prior call. Unless there is an ERROR in communications, the interrupt said there was data there - would this be a reason to alter the logic for Update/Propagate?

Here is the relevant code change in \uNavINS_CB_Ver6\uNavINS_CB_Ver6.ino::
Code: 
// read the IMU sensor
[COLOR="#FF0000"]      // Imu.readSensor();
[/COLOR]      [U]float ax; float ay; float az; float gx; float gy; float gz; float hx; float hy; float hz; bool MagD;[/U]
      [B]Imu.readSensorPMD( &ax, &ay, &az, &gx, &gy, &gz, &hx, &hy, &hz, &MagD);[/B]

      //Serial.println("IMU sensor read");
      //Serial.println(Imu.getGyroX_rads()*R2D);

      if ( MagD ) Mcnt++;  // DEBUG print values showing when Mag Diff was detected!
      else McntN++;

      Filter.update(uBloxData.iTOW, uBloxData.velN, uBloxData.velE, uBloxData.velD,
                    uBloxData.lat * D2R, uBloxData.lon * D2R, uBloxData.hMSL,
                    ax, ay, az, gx, gy, gz, hx, hy, hz );
[COLOR="#FF0000"]//                    Imu.getGyroX_rads(), Imu.getGyroY_rads(), Imu.getGyroZ_rads(),
//                    Imu.getAccelX_mss(), Imu.getAccelY_mss(), Imu.getAccelZ_mss(),
//                    This_Mag[0], This_Mag[1], This_Mag[2]);
[/COLOR]
 
Tim - from my point of view yes. I have used it for other purposes like sensor calibrations - there does tend to be a temp dependency for the gyro and a little for the accel. So from my point of view I would leave it in.

In Brian's old library there used to be a function call to readSensor9 just to get the accel/gyro/mag data.

BTW. To be honest I prefer your method to get the 9 values instead of the individual calls.

Mike
 
Don-Tim

I incorporated the changes from post #493 and all is running fine from my end. I am seeing a couple of things with this run:
1. 10m variation in altitude based on the GPS.
2. 6 degree variation in yaw

Just thought you would like to know
 
Mike,

I've been seeing some issues with the version (Ver6) I've been running. Heading drifts off after a short while, and I was seeing "spikes" in the residuals plots. Was starting to wonder if maybe the dt value was getting corrupted by interrupts or something.

So your new version is looking fairly stable? Is heading staying consistent for a while? How about pitch and roll? Can you post a copy so I can see it?

I was even thinking about going back and checking out some previous versions to see if they looked more stable than this current one, but thinking I'll wait and check your version out.

Don
 
Don

Think the original version you posted yaw was a lot more stable. I don't worry about heading too much as it is directly related to yaw - so whatever changes I see in yaw happens in heading.

Yaw stays stable for a few seconds then it seems to go off to one of the xtremes +2 or -2 degrees from an average value. Seems to deviate as the velX changes to much that was why I was saying if it goes below a certain value just make them zero and see what happens - have to give that a try. Haven't seen the spikes yet but didn't run it for long long durations.

Do you want a copy of the sketch. Will have to modify it slightly since you don't have a MS5637 :) or do you just want to see a couple of plots.

Mike
 
Mike,

Maybe just a couple of plots would be nice.

Other than adding the MS5637 code, what else did you add?

Think I'll take a quick look at some of the earlier versions to see if they were more stable. Also might start trying some simple tests on the Marmot board, see if I can get that up and running. Believe it uses SPI for the MPU9250 interface.

Don
 
Hi Don
The only other I added was the code that Tim just posted as well as a couple of additional options to send different packet's. It defaults to your 77 variable case. I document it if you want me to post it.

I have to go do a couple things on the house but I can run the plots later.

Mike
 
mjs513 said:
Don-Tim

I incorporated the changes from post #493 and all is running fine from my end. I am seeing a couple of things with this run:
...
Click to expand...

mjs513 said:
Tim - from my point of view yes. I have used it for other purposes like sensor calibrations - there does tend to be a temp dependency for the gyro and a little for the accel. So from my point of view I would leave it in.

In Brian's old library there used to be a function call to readSensor9 just to get the accel/gyro/mag data.

BTW. To be honest I prefer your method to get the 9 values instead of the individual calls.

Mike
Click to expand...

Cool - glad I posted a complete working update of the diffs! Are you watching your Loop#? Per my posts it is a good ready reference for catching time cost/value of changes - esp. on your T_3.5.

Calling Imu.'variable'() 9 times bugged me since I saw it - this is a great excuse to fix that - a shame the 16 bit MAG counts change a trivial bit - could have saved a bit more Math*3*(1/SRD)

Would be trivial to grab the _t:: adjusted Temp Value - would have just added it if not hrs late for sleep.

The noted 'readSensorCounts()' might be the function you saw as readSensor9()? That is what I used because if just passed the 9 pointers to get the 9 read values back. Was expecting I'd make a struct to pass - but that was there and easy to copy.

Currently it seems each MAG change in data already incorporated in the "Filter.update()" above? So there doesn't need to be a special case for Mag_Delta? Is that as desired?
 
You must log in or register to reply here.
Back
Top