Forum Rule: Always post complete source code & details to reproduce any issue!
Page 3 of 28 FirstFirst 1 2 3 4 5 13 ... LastLast
Results 51 to 75 of 677

Thread: uNav INS

  1. #51
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    414
    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

  2. #52
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    2,960
    Don

    Try this site to get your data: https://www.ngdc.noaa.gov/geomag-web/#igrfwmm

    By the way wrapped serialprint in once of sincePrint tests so it prints every 10ms, works fine. Lowering the number to see where it hangs.

    Mike

    Edit: 5ms hangs, 10ms is ok.

  3. #53
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    414
    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

  4. #54
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    2,960
    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 by mjs513; 03-28-2018 at 02:19 AM.

  5. #55
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    414
    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

  6. #56
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    414
    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

  7. #57
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    414
    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.

  8. #58
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    2,960
    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

  9. #59
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    2,960
    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.

  10. #60
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    414
    Great, glad you got it up and running!!

    I've had it running for an hour, virtually no heading drift...

  11. #61
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    414
    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

  12. #62
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    8,014
    Quote Originally Posted by Don Kelly View Post
    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.

  13. #63
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    414
    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!

  14. #64
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    2,960
    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

  15. #65
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    2,960
    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
    Attached Files Attached Files

  16. #66
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    8,014
    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 by defragster; 03-30-2018 at 06:34 AM.

  17. #67
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    8,014
    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();
    }

  18. #68
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    2,960
    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.

  19. #69
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    8,014
    Quote Originally Posted by mjs513 View Post
    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

  20. #70
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    2,960
    "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.

  21. #71
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    2,960
    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

  22. #72
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    8,014
    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.

  23. #73
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    414
    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...

  24. #74
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    2,960
    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.

  25. #75
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    8,014
    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

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •