uNav AHRS

Status
Not open for further replies.
holy cow... I just turned off my 5Hz plotting loop to see if TelemetryViewer could handle plotting the full 100Hz serial port stream (SRD=9), and it's plotting it all I think. Whoa... data's going by FAST....

TelemetryViewer, is cool. Wish it could also send serial commands BACK to the Teensy3.6 while plotting. Can you imagine if we could reset R and Q on the fly while looking at the states and innovations. THAT would be slick!
 
Tim,
Probably dumb question here... but here goes...

TelemetryViewer can't be run with the Serial Port open. So I'm not seeing how we'd 1) wire it up, and 2) how we'd view a serial display to send the commands. Would we use a FTDI board to connect to a unused T3.6 serial port, bring that into a USB port? And would the Arduino IDE be able to connect a serial terminal to that second port, or would we need a different IDE like TyCommander?
 
Last edited:
Hi Don. Think you can just hook say SerialX to SerialY on the two teensies for communications and then just use the USB ports on ports. Either use the Arduino serial monitor or use putty for the one you want to use to transmit commands.
 
I tested this to work - hopefully the comments below are right to make it usable for others? One sketch below - under "#if 0" are the parts needed for the Teensy under test to Bypass the Tviewer ownership of the USB port. The IMU INT pin will have to come off Pin 1 to use Serial1.

If you don't have a Second Teensy - buy some more: An FTDI adapter can work - though they won't run at 2 Mbaud. Teensy can run faster - but too much output will overwhelm the default Serial# buffers if not careful, though Proxy Teensy will have no trouble keeping its end empty.

Second Teensy PROXY { BBoard is T_3.6 with onehorse MPU9250 on under pads, GPS is Serial2 and PPS is Pin6, second Teensy is Serial1 with pins 0&1 crossed with GND connected }:
20180123_144442.jpg

Code:
// For second Teensy connect USB to SERMON (TyCommander)
// Connect two Teensy GND's and cross Rx/Tx to either Serial 1 or 2 port here and in the other sketch.
#define PROXY_SER Serial1
#define PROXY_BAUD 2000000
void yield() {}; // Void yield() to bypass default PJRC yield overhead

void setup() {
  Serial.begin(9600);
  while (!Serial && millis() < 4000 );
  PROXY_SER.begin( PROXY_BAUD );
}

void loop() {
  char inChar;
  if ( Serial.available() )
    PROXY_SER.print( inChar = Serial.read() );
  if ( PROXY_SER.available() )
    Serial.print( inChar = PROXY_SER.read() );
}


#if 0 // These are the PARTS for the Teensy Under Test
// On second Teensy set a usable Serial#, Conenct GND and Cross those Rx/Tx to the Proxy Teensy
// I use TyCommander - select the PROXY Teensy to see Proxy output
// Disable Serial on Teensy Under Test to allow Tviewer to connect
// I also do TyCommander [Close IDE and TeensyLoader] "Tools / Integrate to Arduino" and TyCommander allows
// Sending code to the Teensy of your choice.
#define PROXY_SER Serial1
#define PROXY_BAUD 2000000

// Add to setup()
PROXY_SER.begin( PROXY_BAUD );
PROXY_SER.println("\n"__FILE__" "__DATE__" "__TIME__);

// Add to loop() to Parse input from second SerMon
if ( PROXY_SER.available() ) ProcessProxy();

// As desired to print to second Teensy for display on its USB SerMon
PROXY_SER.print( "Hello World" ); // As send output to Proxy port

// Place this function in the sketch and modify as desired.
void ProcessProxy() {
  // read and react to characters from second PROXY SerMon
  char inChar;
  static uint32_t foo = 100;
  while ( PROXY_SER.available() ) {
    inChar = PROXY_SER.read();
    PROXY_SER.print( inChar );
    if ( 'h' == inChar ) {
      PROXY_SER.print( "\nHello World!");
      PROXY_SER.println( foo );
    }
    if ( '+' == inChar ) foo++;
    if ( '-' == inChar ) foo--;
  }
}


#endif
 
Last edited:
Very cool! Thanks!!!!

Will check this out tomorrow to see if I can replicate. This will be very powerful if we can see the KF or EKF performance in real-time, and then actually be able to tweak Q (or R) in the process while we look at performance.

So, one way we would do the KF/EKF tuning would be the following. We would set R to be "reasonable" values (often based on the hardware spec, or guess of spec). Then we use the Q values as "tuning knobs" to adjust performance. What we look at is the sigma of the innovations (y-h(x)) to see if they get better (smaller) or worse (larger) as we change the Qs. We would need to check in a static environment, and then also in a dynamic (moving the board all around) environment.

This will be VERY powerful tool if we can get it to work... I've only been able to do this post-mission in my other KF/EFK applications, and often with hours and hours of work, never been able to actually get this to work in a real-time or near-real-time situation.

So... I would also need to add a plot to TelemetryViewer that shows the running value of the sigma (standard deviation) of the innovations... will add that as well after I see if I can replicate defragster's cool proxy usb setup...

Brian, this analysis capability might be really helpful in tuning/debugging the quaternion approaches too...
 
Here is my version of sketch you last posted of uNavAHRS_MPU9250_EulerV2.ino renamed .txt to upload: View attachment uNavAHRS_MPU9250_EulerV2.ino.txt

That should let you see the diffs to get it working with the posted PROXY code on second Teensy. In that I restored the commented group of (5) Serial prints that STREAM BY! : PROXY_SER.print(Filter.getPitch_rad()*180.0f/PI);

Also in there is this that should also be in the prior PROXY code: void yield() {}; // Void yield() to bypass default PJRC yield overhead

If useful somebody might expand the "void ProcessProxy() {" to have a real parser. Seeing above spew it might be nice to disable/select 'Debug' spew with a switch.

Spare Proxy Teensy will make flying more awkward. I had a more teensy T_3.0 here that easily pins and better ties to the cable, same as a T_3.2. A T_LC would work - but may not be as reliable over 500kbps based on clock math.

Good luck. Given the Proxy code only goes on once - you wouldn't have to change the dev setup. Just button press on the Test Teensy to program it if it isn't first see by TLoader. And with Tviewer taking Test USB - the IDE version may work there. Or you could put it on a second computer.
 
Just to close the loop on yaw and roll, in case anybody else is reading this. To separate yaw from heading just uncomment Brian's commented out code in getYaw_rads() function and delete the current return with the constrain's. For roll all I did was change the return to read return (constrainAngle180(x_(0,0))); from return x_(0,0) and it seems to work. I believe these are the fixes, if not just let me know.

Thanks
Mike
 
Hi Don. Don't want to be the bearer of bad news but you did have the yaw fix in your uploaded zip file. Figured with all the versions its easy to loose the config. I tested the changes in your version and the one Brian posted (v2) as well. Everything seems to working well with EKF. Haven't noticed any drift and heading looks right on as nearly as I can tell. Now I guess its time to start playing with the GPS again. I will have to give Brian's ublox library a try, I've been using NeoGPS for most my projects other than RTK.
 
Yes, had the yaw correction, but I didn't have that roll mod you mentioned, so I've added that. I've noticed that if the sketch runs for a very long time (like hours), when I come back and look, pitch and roll are at some odd value, like 177 instead of 0. But that was before I added that constraintAngle mod for roll, so maybe that was it. If not, we might still have some singularity or maybe zero-crossing tweak we need to make.

I had tried NeoGPS as well, works nicely. I've switched over to Brian's library and it was running great.

I'm taking a step back right now and trying to send commands from one T3.6 to another T3.6 via Serial, using two different computers. Should be simple, but this is new to me, so am wiring a simple test up now. This will allow me to change EKF parameters on the fly (on one computer) and see the results via TelemetryViewer (on the other computer). At least I think so...
 
Hi Don. What you are doing should work, I don't see why it should. Principal is the same as what defragster is doing and what I have tried in the past. Only difference is you are on two computers.
 
I'm now able to send commands from a second T3.6 (via second computer) to the T3.6 with the EKF. So suspect tomorrow sometime I'll be up and running with being able to adjust Q and R in real-time while viewing TelemetryViewer in real-time.

When adjusting Q and R, one key thing is to be able to see how you've affected the KF/EKF innovations. I usually watch the 1- or 3-sigma of the innovations to see if I've made them go up (bad) or down (good). So I'll also need to add a routine that calculates the sigma of the innovations on the fly.

enough for today...
 
I'm now able to send commands from a second T3.6 (via second computer) to the T3.6 with the EKF. So suspect tomorrow sometime I'll be up and running with being able to adjust Q and R in real-time while viewing TelemetryViewer in real-time.

When adjusting Q and R, one key thing is to be able to see how you've affected the KF/EKF innovations. I usually watch the 1- or 3-sigma of the innovations to see if I've made them go up (bad) or down (good). So I'll also need to add a routine that calculates the sigma of the innovations on the fly.

enough for today...

If you look at the top of the update function, you’ll find a good recursive routine I’m using to compute the IMU variance. Should be able to apply that to the innovations.

EDIT: you’ll want to reset the variance states whenever you send a command to update the EKF so you get the variance of the updated values.
 
defragster/brian

Just hooked up my m8u using Brian's library and it works like a charm. I also tried using Teensythreads to read the GPS and do the printout in the main loop (can do anything then with it at that point). It works but had to add an update flag to make sure I wasn't printing if the data wasn't read. If you want to give it a try to see if I missed something here is my test code:
Code:
#include "TeensyThreads.h"
#include "Streaming.h"
#include "UBLOX.h"
#include <Wire.h>

// a uBlox object, which is on Teensy hardware
// serial port 3
UBLOX gps(1);

// the uBlox data structure, UBX-NAV-PVT
gpsData uBloxData;

struct GPS {
  unsigned long   iTOW;        ///< [ms], GPS time of the navigation epoch
  unsigned char   utcDay;      ///< [day], Day of month, range 1..31 (UTC)
  unsigned char   utcHour;      ///< [hour], Hour of day, range 0..23 (UTC)
  unsigned char   utcMin;     ///< [min], Minute of hour, range 0..59 (UTC)
  unsigned char   utcSec;     ///< [s], Seconds of minute, range 0..60 (UTC)
  long            utcNano;      ///< [ns], Fraction of second, range -1e9 .. 1e9 (UTC)
  unsigned char   fixType;      ///< [ND], GNSSfix Type: 0: no fix, 1: dead reckoning only, 
                                  ///< 2: 2D-fix, 3: 3D-fix, 4: GNSS + dead reckoning combined, 
                                  ///< 5: time only fix
  unsigned char   numSV;      ///< [ND], Number of satellites used in Nav Solution
  double          lon;        ///< [deg], Longitude
  double          lat;        ///< [deg], Latitude
  double          hMSL;       ///< [m], Height above mean sea level
  double          velN;       ///< [m/s], NED north velocity
  double          velE;       ///< [m/s], NED east velocity
  double          velD;       ///< [m/s], NED down velocity
  double          gSpeed;     ///< [m/s], Ground Speed (2-D)
  double          heading;      ///< [deg], Heading of motion (2-D)
  double          pDOP;       ///< [ND], Position DOP
  bool            upDated;
} rtk;


Threads::Mutex drdy_lock;
int IMU, GPS;

#define cout Serial

void setup() {
  // serial to display data
  cout.begin(115200);
  
  // starting communication with the GPS
  // receiver at 115200 baud
  gps.begin(115200);
  Serial1.setRX(27);
  
  // put your setup code here, to run once:
  GPS = threads.addThread(readGPS);
  threads.setTimeSlice(0, 1);
  threads.setTimeSlice(GPS, 50);
  if (threads.getState(GPS) == Threads::RUNNING) Serial.println("GPS thread started");
  rtk.upDated = false;
}

void loop() {
  if(rtk.upDated) {
    cout << rtk.utcHour << "\t" << rtk.utcMin << "\t" << rtk.utcSec << "\t" << rtk.utcNano << " ---->>  ";
    cout << "Lat: " << _FLOAT(rtk.lat,10) << "  Lon: " << _FLOAT(rtk.lon,10) << endl;
    cout << " ------------------------- " << endl;
    rtk.upDated = false;
  }
}

void readGPS(){
 while(1){
 { Threads::Scope scope(drdy_lock);
      if(gps.read(&uBloxData)) {
        rtk.iTOW = uBloxData.iTOW;
        rtk.utcDay = uBloxData.utcDay;
        rtk.utcHour = uBloxData.utcHour;
        rtk.utcMin = uBloxData.utcMin;
        rtk.utcSec = uBloxData.utcSec;
        rtk.utcNano = uBloxData.utcNano;
        rtk.fixType = uBloxData.fixType;
        rtk.numSV = uBloxData.numSV;
        rtk.pDOP = uBloxData.pDOP;
        rtk.lat = uBloxData.lat;
        rtk.lon = uBloxData.lon;
        rtk.heading = uBloxData.heading;
        rtk.gSpeed = uBloxData.gSpeed;
        rtk.hMSL = uBloxData.hMSL;
        rtk.velN = uBloxData.velN;
        rtk.velE = uBloxData.velE;
        rtk.velD = uBloxData.velD;
        rtk.upDated = true;
      } 
  }
 }
}

Good night all.
 
Great News Don, glad that worked out. You could run a second copy of Tviewer there too perhaps alternating with debug - have it show those stats in human form - send CMD to change if desired - then own that port on Mac with Tviewer - disconnect and adjust ... repeat.

I compiled the xxx_EulerV2 code for T_3.6 at 48 MHz and it runs Tviewer and Proxy output at 250 Hz.

I compiled at 24 MHz and the 2Mbaud Proxy output fails - I need to ask about that - but turning that off - 250 Hz output to Tviewer looks to be working!

The math so far integrating and reading 250 IMU data sets per second can run on 24 MHz ( compiled Fastest w/LTO ) - while streaming the Tviewer data out USB Serial for plotting!

Tapping the surface under the Breadboard shows the acceleration quite vividly!


@mjs - I pulled down the latest TeensyThreads with some idea of giving it a try - not yet started - all my coding was UniThread - need to read up on Threading
-- Don't know the time allocation units from looking but with uBlox sending 100 characters at the 460800 baud I used it takes about 2.17 ms for total transit.
-- The 115K rate would be about 4 times that or 8.5 ms occurring in Chunks once per transmission - I was using 5 Hz - so every 200 ms.
-- Once buffered the transfer is a small portion of that and the Parsing did not seem to add any significant time.
-- In the end it won't need 1ms to process each string. The parsing is very efficient- and polling doesn't waste too much time.
---- I have isr() code that detects ( and time stamps) the start bit of the 1000 bit stream in the 100 byte message. That could be used to activate the polling thread every ~195 mseconds.

> I don't know the effect of "Threads::Scope scope(drdy_lock);" and I don't see any other actors on the drdy_lock? But instead polling on 'rtk.upDated'?
> The gps.read() pulls any arriving/buffered bytes and goes TRUE when MSG is completed with valid checksum.
> I had only gotten as far as expecting to see something like this - where the GPS thread would yield after any call to gps.read():
Code:
  bool readReturn;
 [U]WHILE[/U] ( !( readReturn= gps.read(&uBloxData)) ); // this will exhaust buffer of any corrupt or partial messages until a completed message is started/found.
  if( 0 != readReturn ) {
    // Message_Completed
    // transfer data code
    SIGNAL.Data_Ready // whatever the mechanism is to have another thread HALT until a condition is met - this would release that
    YIELD() // LONG ( pause the thread appropriate time ) - it will be over 190 ms before the next message, no reason to poll - can wait for a SIGNAL if the INT_isr() for MSG start bit can be used.
  }
  else {
    YIELD()   // SHORT ( just abandon remaining timeslice, but return soon ) - mid transmission 
  }
 
@defragster.

Took part of your advice anyway. Don't think there is a need to halt or even pause other threads. I ran a test case where I incorporated the IMU and GPS in separate threads. and just did the printout in the main loop. Whenever the IMU was read it checked for an update to the GPS. The time difference is about a hundredth of a second. Of course I am working on this while my eyes are still have closed :). Anyway if you want to check it out I am attaching a zip of the sketch. I have the GPS on Serial1 and IMU on SPI. Haven't added Tviewer and there is still things to be done but its a start.

Mike
 

Attachments

  • ubloxGPSThread.zip
    2.9 KB · Views: 124
I apologize for the number of posts but the ability to edit is still restricted. Anyway I am attaching a slightly updated sketch that outputs GPS/IMU data to the Telemetry Viewer. I also included the template for Tviewer if you want to play with it.
 

Attachments

  • ubloxGPSThread.zip
    4 KB · Views: 129
Nice Mike! I hope to take a look at it later tonight.

I've got my version of the sketch calculating mean and sigma of the innovations using the QuickStats library. Works like a champ. Working on getting those values fed into TelemetryViewer. Then will link in the commanding of Q levels from the second computer. Will post when I get a version running, maybe with a photo to show my T3.6 hookups.

Got meetings the next several hours tho...
 
Thanks Don. Can't wait to see what you came up with. BTW. Good luck with your meetings, I remember those days well.
 
Mike, I don't see the TViewer.txt in the last .zip?
Where does #include "Streaming.h" come from? { commented out and it compiles and runs }
Used Don's: uNavEulAHRS.h

Looking at #define'ing your sketch to fit my system to look at TThreads - seems we all have alternate hardware configs :( : Wire .vs. SPI, Alt RX on GPS, GPS and Debug Serial swapped, IMU int pin moves, SDA/SCL moves.

I have your code running with #define adds and associated code. Including pushing i2c to 1 Mbit!

You are right without Serial Polling in loop 1 million times each second - but only once per timed thread change when empty it won't be as wasteful when it then yields, and better not to sleep through incoming character. Under TTthreads the change to 'void yield()' does not apply as it is already neutered AFAIK.

It seems the timestamp for IMU data should be recorded during the isr(). Same can be done on the GPS message start bit, rather then after the message is completed - of course that pin moves too.

For #defines I am coding around these - different names than used in Don's sketch - but to similar effect:
#define IMU_ADDR 0x69 // 0x68 // SPI 9
#define IMU_SCL 47 // 0x255
#define IMU_SDA 48 // 0x255
#define IMU_INT_PIN 50 // 1 // 14
#define coutD_BAUD 2000000 // coutD for DEBUG port
#define coutD Serial1 // Serial2
#define GPS_Start 2 // 1 // UBLOX gps(gps_Start);
#define GPS_PORT Serial2 // Serial1
#define GPS_BAUD 460800
#define GPS_AltRx 255 // 27
#define IMU_BUS Wire // SPI
#define IMU_SPD 1000000 // 0==NULL or other
 
Last edited:
Hi defragster. Its in the second version zip in post #219, gpsIMU.txt, I just did a double check. Think Streaming was a left over from when I was using Mikal Hart's streaming library. Got tired of serial prints all over the place.

One way its good that we are using all different configs, better test for the library. Regarding TeensyThreads, of course using this puts it right back to Teensy specific code unless you are using a ROS.

Same can be done on the GPS message start bit, rather then after the message is completed
Ok I get how to do this for the IMU ISR. Not sure what you mean "start bit" for the GPS.
 
Here is my edited version of the posted code I got: View attachment ubloxGPSThread.ino.txt
:: includes various #defines, and the Serial Rx_ISR() in some fashion - updated output below

GPS Start bit detect is in some sketch here not shared yet. It might look something like this:
Code:
#define GPS_SRX 9  // Must be set to Serial# Rx Pin #
#define GPS_BAUD 460800
// FASTRUN void GPS_serialrx_isr()
volatile uint32_t GPS_RX_time = 0;
volatile uint32_t GPS_newData=0;
FASTRUN void GPS_serialrx_isr() {
  static uint32_t tt;
  static uint32_t xx = 0;
  tt = micros();
  if ( tt - xx > ((F_CPU / GPS_BAUD) * 30) ) {
    GPS_RX_time = tt; // Start bit transition detected after 'idle' time
    GPS_newData=1;
  }
  xx = tt;
}

// In setup()
  attachInterrupt(digitalPinToInterrupt(GPS_SRX), GPS_serialrx_isr, RISING);

For IMU the isr() could be altered to do this in some legitimate manner - otherwise the time stamp will vary depending on when the readIMU() gets hit next:
void runFilter() {
newData = 1;
rtk.ts = ((double) timeStamp) /3600000.0f;
}

Indeed - good we are using alternate hardware configs - would be nice to have a uniform set of #define to quickly make any shared sketch work. Also yes, if done in TThreads - it would take some rework to make it a normal sketch again. That's why I wasn't rushing after seeing that so far everything is running on a 24 MHz CPU.

BTW: Paul confirmed that at 24 MHz - 2 Mbaud is not supported - only runs up to 1.5 Mbaud.

I found the STREAM stuff - use was commented out - I converted to .print for GPS and it works to show GPS parsed data:
Code:
// ...
        coutD.print(" Lat: ");  coutD.print(rtk.lat,7);
        coutD.print(" Lon: "); coutD.print(rtk.lon,7);
        coutD.println(" ------------------------- ");
Lat: 46.6143829 Lon: -123.6503630 Time: 183678 -------------------------
13.42 -163.45 120.89 120.93 0
13.42 -163.46 120.91 120.92 0
13.41 -163.46 120.92 120.92 0
13.41 -163.47 120.86 120.91 0
Lat: 46.6143829 Lon: -123.6503630 Time: 183874 -------------------------

Indeed .print() is cumbersome - it always comes in pairs for 'label='value so I started using a macro like this: #define SP2( A, B ) { Serial.print(A); Serial.print(B); }
 
Last edited:
Thanks for the code and explanation defragster. Makes absolute sense when you see doesn't it. Always learning something.

it would take some rework to make it a normal sketch again. That's why I wasn't rushing after seeing that so far everything is running on a 24 MHz CPU.
Hate to say this, but I have been moving away from most of the Arduino boards. Being replaced by the Teensy 3.2 or the 3.5. Still like the Mega and Curie though. Haven't used them for quite awhile since I figure out a way to get a mega type footprint for the 3.5 :).
 
Status
Not open for further replies.
Back
Top