uNav INS

Brian,
I've converted over the "coremag" mag routine, which is called by the 15-State with magnetometer EKF. The 15-State uses the field[3] to field[5] values in the update stage. These three values are supposed to be the "local magnetic variation given position, altitude, and date." By any chance can you tell me if the numbers below seem about the right size, i.e. order of magnitude? I'm still searching the references to see if I can find that out, but no luck thus far.

JD is 2440553
field[0]: -45383.26
field[1]: -25598.36
field[2]: 3398.72
field[3]: 25728.97
field[4]: 3398.72
field[5]: 45309.34

Don
 
Mike,

Glad it's running!

Good call on the web site.... looks like my mag field numbers are in the ballpark... very cool. Now I need to add the mag calls into the 15-State and see if it all works!

Don
 
Hi Don
Glad the site helped. Been using it for a while to get the declination and check my mag readings. :)

PS. Think the problem is really with the MPU9250. It finally cut out after about 5 minutes of operation. I noticed when I was doing some temperature calibration on the gyro's it would seem to cut out when it got over 30deg C. Have to check on that.

Mike

EDIT: Issue is the interrupt pin. Don't really need it anyway....
 
Last edited:
Mike,
Looks like I (may) have the mag-version of the EKF running. Am cautiously optimistic

Will test it for a bit, and assuming it looks good, will upload it here...

Don
 
View attachment uNavINS_MPU9250_I2C_MAGV1.zip

Here it is. Uses the same TViewer file I posted a few days ago.

I've run it for about 30 min and it seemed very stable. So like I said, cautiously optimistic...

Mike - you asked about a parameter to pull out of the EKF and stream to SPI, as a test case. Anything would work, how about the sqrt of the diagonals of P? This will be cool!!

Don
 
I'm noticing some things that are probably related to tuning deficiencies. For example, it seems like if I move it too soon after bootup, before the heading seems to have converged, it goes haywire. So definitely some initialization, gains, or sensitivity issues still.
 
Hi Don
Sorry disappeared for a while. Looking forward to giving it a test drive. Going haywire before convergence - that one doesn't surprise me - like you said going to have to tune it up a bit.

Mike
 
Ok Don.

Downloaded and running. Yaw a lot better. More like what I was seeing with the Watson implementation. Had it running for little bit and it seems to staying within a 1.5 degree band. The good thing is when it does start to increase you see it correcting to come back down.

Now for the other work.
 
Mike,
Every so often, if it's left running for 30 min or so, the heading and yaw estimates seem to blow up. I'm guessing this might be caused by spurts of noisy data, maybe easily corrected by watching for it and removing spurious data. But just a guess for now...

Don
 
Mike,
Every so often, if it's left running for 30 min or so, the heading and yaw estimates seem to blow up. I'm guessing this might be caused by spurts of noisy data, maybe easily corrected by watching for it and removing spurious data. But just a guess for now...

Don

Is that with a static board? Is that 'blow up' attributable to a single IMU hardware part - Accel/Mag/Gyro ?

Filtering each Accel/Mag/Gyro independently could cause it to reject/diminish true movement. Wondering if there is an easy way to quickly quantify apparent change in each part and use that to filter out changes that aren't in agreement? "Two out of Three Space shuttle computers agree that the third one should be ignored". Gosh that was easy to type . . . detecting and properly adjusting for that likely a bit more complex.
 
Tim,
Until I can tap into the inner workings of the EKF I'm not sure the cause. From past experience, I've seen data "glitches" cause spurious values that can get fed into the EKF. So I'm not really talking "pre-filtering" per se, it's really just keeping an eye out for obvious bad data and then rejecting that particular instance of bad data. But really just a guess until I start digging into the inner workings of the EKF and get some tuning done.

As I was typing the EKF crashed again. But it had been running for 45 min or so just fine. So something's up with it!
 
Hi Guys,

I've seen this as well. When looking at the raw data all of sudden you see spikes in the data coming from the 9250. Only looked at accel and gyro, didn't really look at mag at the time.

Mike
 
Got it working thanks to Tim. Anyway here are the updated files with 36 doubles sent to the slave from the master. Includes the sqrt of the diagonals per Don's request and the quaternions being dumped.

Don, I'll let you work the Tviewer file :)
 

Attachments

  • uNavINS_MPU9250_I2C_MAGV1.zip
    19.5 KB · Views: 136
  • TVslaveDK.zip
    2.1 KB · Views: 120
Downloaded - will power up my long disused system and see how it goes . . .

<edit> Only compiled once to THEN change the configDefines. It gets through 'SetupComplete' and is doing pinToggle - and when slave goes offline I get retry notes.

Not sure - what's wrong just yet - possibly bad calibration? Need to run those proper SPI wires to make it more movable.

<edit2>: if (uBloxData.numSV > 5) {
My FIX is failing where it used to work - I got a short fix and I did see some data. Moved back to window . . . waiting ...
 
Last edited:
GPS FIX arrived closer to window and all is well, after Slave restart? First printed Slave #'s were : "1_good_Number, NAN's,..." . Reset Slave and got valid values with change on Motion - so my onehorse MPU9250 running fine again at 3.4 MHz i2c.

Something is way more finicky ( is it raining lightly ) - losing my RED FIX LED where it used to work? ... moved from Window and I get: " WARNING: Apparent loss of IMU data !!" - Slave output stopped - no notice of FIX LOST - that caused slowed IMU attention.

Slave pinToggle - running fine. Bad trouble holding FIX - even in Window.

Mike: You have Math [ 9 copies of "*R2D"] on Master before shipping to Slave - doing that on Slave with dtostrf()/formatting will maximize Master gain with SPI_MST.

I see that the TViewer output is now 413 characters long! That would cost even more USB time/distraction on the Master! Teensy USB should still be good to TViewer even at 1,000 Hz ( Hah! ) it is well under 50% capacity.

The millis()%100 for the SPI HeartBeat is fun - but costing up to 10 ms/sec with delay(1). Also that rate seems panicky - and some more SPI time too. This will FLICKER FAST (dim) on powerup - but then pinToggle each 200 ms. When GPS is lost it will blink 100 ms faster so slave will show PANIC then - at least in theory - I can't see FIX to test it going slow :( I added an adjustment for loss of IMU too - PinToggle will go even faster

NOTE: When I unplug my SLAVE to put MASTER in window it gets FIX until I replug Slave? Either power loss as cabled/wired or the Slave is interfering with GPS? I just put SLAVE on USB battery pack - I have Fast pinToggle and no GPS FIX yet? Unplugged Slave and got FIX - either gets lost or under 6 SATS and fast blinks - with some slow 200 ms blinks as expected. Will rewire SLAVE and reposition and report back. I never saw this before ...

NOTE2>> Dropped SPI to 24 MHz from 30 MHz and FIX comes/stays better ... trying 12 MHz ... maybe better but not good. My 'IMU_SRD 19' is low IMU updates. Reprogrammed slave T_3.5 to 120 MHz with 24 MHz SPI - still more off than on ???

Mike: Any chance this 'loss of fix' is what you saw as failure to get IMU data?

Code:
elapsedMillis HeartBeat;
void loop() {
  if ( HeartBeat > 200)   {
    aHRS.pinToggle(LED_BUILTIN);
    HeartBeat -= 200;
  }

  //Define RTC time for this iteration
  rtcTime = (float) micros() / 1000000.0 - rtcOffset;

  gps.read(&uBloxData);
  //Serial.println("Read GPS");
  //Serial.println(uBloxData.numSV);

  // If GPS receiver is locked to at least five sats and if there is new
  // IMU data, then run the EKF. This seems to run at about 50Hz (20ms samples).
  if (uBloxData.numSV > 5) {
    //Serial.println("numSV>5");

    // Print out warning if apparent loss of IMU data
    if ((rtcTime - newIMUDataTimer) > 0.150) {
      Serial.println(" WARNING: Apparent loss of IMU data !!");
      if ( HeartBeat < 150)   {
        HeartBeat += 150;
      }
    }

    if (newIMUData == 1) {
      //Serial.println("newData=1");
      newIMUData = 0;
      newIMUDataTimer = (float) micros() / 1000000.0f - rtcOffset;

      // read the IMU sensor
      Imu.readSensor();
      //Serial.println("IMU sensor read");

      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(),
                    Imu.getMagX_uT(), Imu.getMagY_uT(), Imu.getMagZ_uT());


      // Print values to serial
      serialPrint();

    }
  }
  else if ( HeartBeat < 100)   {
    HeartBeat += 100;
  }
  aHRS.events();
}
 
Morning Tim
You have Math [ 9 copies of "*R2D"] on Master before shipping to Slave
Easy enough to fix.
The millis()%100 for the SPI HeartBeat is fun
Like you said it is fun. See you added a heartbeat to the loop :)
Mike: Any chance this 'loss of fix' is what you saw as failure to get IMU data?
Don't think so. When I was debugging I saw the same thing when I ran a basic I2C sketch just for the IMU.
When I unplug my SLAVE to put MASTER in window it gets FIX until I replug Slave?
Not sure why its taking so long to get the fix, I haven't seen that one.
 
Morning Tim
Easy enough to fix.
Like you said it is fun. See you added a heartbeat to the loop :)
Don't think so. When I was debugging I saw the same thing when I ran a basic I2C sketch just for the IMU.
Not sure why its taking so long to get the fix, I haven't seen that one.

> Hi - yeah - easy fix - maybe not significant - but until the slave CPU gets overloaded everything over there is a gain
> 10 pinToggles was a bit much - so slower heartbeat is enough to see it run - and then going faster can alert of issues at a glance. A one off debug spew 'lost Fix' might be nice - I had to read code to see why no TViewer prints
> Odd on your IMU - was hoping the false message on loss of fix may have mislead you. Interesting the GPS is required to have 6 SATS to proceed

>> The fix comes okay, but not when Slave is powered? Same setup I used early March - but now powering Slave stops or interrupts the FIX ????
WORKING NOW with Slave? : I restored a shortened debugGPS() to track it and now I'm seeing fix==3 and SATS=7 to 15?
- fix took some minutes - right at window - no rain today
- Num SATS 'seems' to trend higher with Slave not running. I'm seeing 10-15 SLAVE OFF, was 8-10 Slave ON. Will update wiring.

NOTE: A couple ifdef's like this are needed in main.ino for compile without TViewer:
Code:
#ifdef TViewerSPI
  aHRS.events();
#endif
 
"Num SATS 'seems' to trend higher with Slave not running. I'm seeing 10-15 SLAVE OFF, was 8-10 Slave ON. Will update wiring." - that's kind of strange. Didn't print number of sat's so I can't say for sure. But the GPS runs independent of Teensy unless there is incomplete GPS data. Late at night I seem to have problems with getting a fix quickly but during the day its pretty quick.

"I had to read code to see why no TViewer prints" - sorry about that - I didn't have it print anything if it is sending data to the slave. Figured could use Serial for something else that we wanted to monitor or check later on.

" Interesting the GPS is required to have 6 SATS to proceed" - think the test is for numSV > 5 so it would need 6 to pass. In the past I used the Fix field as a test so even if you had 4 sats but had a 3d or better fix (RTK) for instance you were fine. Maybe we should go back to that.

" A couple ifdef's...." - will go through the code and add.
 
Hi Don,
Quick question. Tim reminder me of something that I wanted to ask. Is there a way to run the filter without GPS so if you loose GPS you don't loose your attitude and heading. Right now if you loose GPS it just stops updating. On GPS failure you would still have heading and could project a course from your last known good position.

Mike
 
Running NOT TViewer and then powered Slave and the higher SATS count persists - My wonderment was if the SPI Transfers were making noise for the GPS :: Seeing 14 to 16 past 15 minutes

>Slave was Ready - doing re-enable of TViewer :: Dropped in the next minute down to 9 to 11 - is getting back to 11-13 - some 14's
Return !TViewer quickly reads steady 15 with some 14 and even more 16's now.

The code is fine to have min 6 SATS - was just scary when my system showed no output to TViewer with first run of this new code with no FIX. GPS data is jumpy with lots of SATS - can only be worse with fewer.
 
Sorry Mike, am a day behind in reading posts... will try out your version first thing Sat morn!

Yes... with no GPS it should just propagate (time step) without the update step, and uncertainty just grows. I'm getting ready for a conference call, so will have to re-look at the code to see what's up.

I want to look at your version when I get a chance, then later want to go add back in the code that Brian had (in uNavAHRS) that auto-calculates the IMU means and standard deviations...
 
Don,
"Yes... with no GPS it should just propagate (time step) without the update step," - the way you have it set up is no updates occur unless you have 5 or more satellites. Take a look and you will see what I mean.

Tim
I just hooked mine back up and am getting steady 7-8 satellites. For me that's about normal for my environment. Sometimes I will get 9-10 depending on the time of day and what I have set up in the GPS for accepting satellite inclination.
 
When you do a test !(TViewer) build see what you get? SPI Wires run under my breadboard to Slave with GPS taped on top. I am seeing it pick up more just now SATS==16 and 17 with !TViewer - Back to TViewer and down to 13 and 14.

Here's the shorter debugGPS() I put in main.ino:
Code:
loop() {
   //...
      if ( gps.read(&uBloxData) ) debugGPS();
   //...
}
#ifdef coutD
void debugGPS( )
{
  static uint32_t GPScnt = 0;
  static unsigned char lastT = 'a';

  GPScnt++;
  if ( lastT != uBloxData.utcSec )
    coutD.print( "\n-----" );
  coutD.print( "\t" );
  lastT = uBloxData.utcSec;
  coutD.print( "\t #" );   coutD.print( GPScnt );
  coutD.print( "  @" );   coutD.print( uBloxData.utcHour);
  coutD.print( ":" );   coutD.print( uBloxData.utcMin);
  coutD.print( "." );   coutD.print( uBloxData.utcSec);
  coutD.print( "_" );   coutD.print( uBloxData.utcNano);
  coutD.print( " [fix:" );   coutD.print( uBloxData.fixType);
  coutD.print( " #:" );   coutD.print( uBloxData.numSV);
  coutD.println( );
}
#else
void debugGPS() {}
#endif
 
Back
Top