uNav AHRS
- Thread starter brtaylor
- Start date
- Status
defragster
Senior Member+
Interesting - I think not - here is what my static unit is showing:
My elevation is different (33>35) - SAT counts are updated - oddly the X,Y cords keep same last 3 digits - but are changing.
There is one u-center value that wasn't clear to me - about FREQ - but wasn't sure what that did. If you want to post a u-center screen shot I can try to get the same view if that might help.
this is the code giving that output:
My elevation is different (33>35) - SAT counts are updated - oddly the X,Y cords keep same last 3 digits - but are changing.
There is one u-center value that wasn't clear to me - about FREQ - but wasn't sure what that did. If you want to post a u-center screen shot I can try to get the same view if that might help.
this is the code giving that output:
Code:
void ShowGPS()
{
uint32_t tt = 0; // todo PPS_PartSec();
Serial.print(uBloxData.utcMonth); ///< [month], Month, range 1..12 (UTC)
Serial.print("/");
Serial.print(uBloxData.utcDay); ///< [day], Day of month, range 1..31 (UTC)
Serial.print("/");
Serial.print(uBloxData.utcYear); ///< [year], Year (UTC)
Serial.print(" @ utc ");
Serial.print(uBloxData.utcHour); ///< [hour], Hour of day, range 0..23 (UTC)
Serial.print(":");
Serial.print(uBloxData.utcMin); ///< [min], Minute of hour, range 0..59 (UTC)
Serial.print(":");
Serial.print(uBloxData.utcSec); ///< [s], Seconds of minute, range 0..60 (UTC)
Serial.print("\t");
Serial.print(uBloxData.numSV); ///< [ND], Number of satellites used in Nav Solution
Serial.print("\t");
Serial.print(uBloxData.lat, 10); ///< [deg], Latitude
Serial.print("\t");
Serial.print(uBloxData.lon, 10); ///< [deg], Longitude
Serial.print("\t");
Serial.print(uBloxData.hMSL); ///< [m], Height above mean sea level
SP2( "\t", tt );
Serial.println( " ------------------------ ");
}Thx. Think I found it... in u-center there's a rate setting under configuration, and you can set to 200ms (5Hz). so now my velocities are changing at 5Hz, but time (.iTOW) still stays the same for 5 readings, the entire 1 second. so tomorrow I'll see if other time values might give me the 200 ms increments.
defragster
Senior Member+
My first pass of the RTC offering for timing should be done - but then I went to make the calibrate funcs properly restore.
Is there a limit on setSrd() other than param is a uint8_t : so max value is 255?
I am not recording clean exit from setSrd( 199 ) or for (99) or ( 49) ? But yet a setSrd(199) works AFTER CalibrateGyro() ?? ? ? Because there is no clean exit - the stored _srd is not set to the new value, and my TEMP store for RESET keeps setting it back to ZERO. I have to add more debug to see what error is returned with no clean exit even though the setting works.
> I see CalibrateGyro in .begin() - then being done again in InitIMU() after the stored data is loaded ? Perhaps the Set of the recalled values can't happen until after .begin()?
> During calibration various params are changed - then it enters read loop - should the first readSensor() result perhaps be discarded? If there is data present from old params it could skew the Calibration.
Is there a limit on setSrd() other than param is a uint8_t : so max value is 255?
I am not recording clean exit from setSrd( 199 ) or for (99) or ( 49) ? But yet a setSrd(199) works AFTER CalibrateGyro() ?? ? ? Because there is no clean exit - the stored _srd is not set to the new value, and my TEMP store for RESET keeps setting it back to ZERO. I have to add more debug to see what error is returned with no clean exit even though the setting works.
> I see CalibrateGyro in .begin() - then being done again in InitIMU() after the stored data is loaded ? Perhaps the Set of the recalled values can't happen until after .begin()?
> During calibration various params are changed - then it enters read loop - should the first readSensor() result perhaps be discarded? If there is data present from old params it could skew the Calibration.
Mike,
Thanks, I'll read those PDFs. It turns out iTOW is a "long", so must do (float iTOW)/1000.0 to put it in float seconds. Duh, I shouldn't have missed that...
So, even though I have the uBlox set to 5Hz, now the main loop is producing about 9 readings (instead of 5) per second, and here's an example of the iTOW values in seconds:
571642.0000
571642.0000
571642.1875
571642.3750
571642.3750
571642.6250
571642.6250
571642.8125
571642.8125
571643.0000
So four are duplicates. Also interesting is that the even second increments are all right on ".0000" but the other readings aren't evenly spaced at ".2000" values (5Hz). So like you mentioned, duplicates must be removed, and then I'l just need to allow for uneven time increments.
Removing duplicated gives times of
571642.0000
571642.1875
571642.3750
571642.6250
571642.8125
571643.0000
I'm guessing these times for the GPS are very accurate (although would be nice to have out to ns, not just ms). But the RTC timing for grabbing the IMU's accel NED-inertial will be very important for the KF accuracy!
After I look at Mike's PDFs, I'll post a version of this INS and GPS data collection sketch in case one of y'all want to take a look. Then I need to get out in my truck with the board and collect some data, but we're having torrential rain today...
Thanks, I'll read those PDFs. It turns out iTOW is a "long", so must do (float iTOW)/1000.0 to put it in float seconds. Duh, I shouldn't have missed that...
So, even though I have the uBlox set to 5Hz, now the main loop is producing about 9 readings (instead of 5) per second, and here's an example of the iTOW values in seconds:
571642.0000
571642.0000
571642.1875
571642.3750
571642.3750
571642.6250
571642.6250
571642.8125
571642.8125
571643.0000
So four are duplicates. Also interesting is that the even second increments are all right on ".0000" but the other readings aren't evenly spaced at ".2000" values (5Hz). So like you mentioned, duplicates must be removed, and then I'l just need to allow for uneven time increments.
Removing duplicated gives times of
571642.0000
571642.1875
571642.3750
571642.6250
571642.8125
571643.0000
I'm guessing these times for the GPS are very accurate (although would be nice to have out to ns, not just ms). But the RTC timing for grabbing the IMU's accel NED-inertial will be very important for the KF accuracy!
After I look at Mike's PDFs, I'll post a version of this INS and GPS data collection sketch in case one of y'all want to take a look. Then I need to get out in my truck with the board and collect some data, but we're having torrential rain today...
EDIT to last post...
After looking at the PDFs Mike sent, using UTC (rather than iTOW) is the way to go. Brian's uBlox library parses out the components (weeks, hrs, min, s, ns) of UTC, but looks like I'll need to combine them to get the full UTC value.
Just looking at the ns parameter (utcNano) is interesting. The increments it provides is very close to 0.200 s intervals, which is what I was expecting, still with duplicates, but nicer spacing. more like
-.000378794
.199621159
.3996210992
.5996210575
.7996209860
which is very close to
0.0
0.2
0.4
0.6
0.8
so that's great. now i just need to build the complete utc value and i'll have the gps timing part solved...
After looking at the PDFs Mike sent, using UTC (rather than iTOW) is the way to go. Brian's uBlox library parses out the components (weeks, hrs, min, s, ns) of UTC, but looks like I'll need to combine them to get the full UTC value.
Just looking at the ns parameter (utcNano) is interesting. The increments it provides is very close to 0.200 s intervals, which is what I was expecting, still with duplicates, but nicer spacing. more like
-.000378794
.199621159
.3996210992
.5996210575
.7996209860
which is very close to
0.0
0.2
0.4
0.6
0.8
so that's great. now i just need to build the complete utc value and i'll have the gps timing part solved...
Here's a release of a version that writes both GPS and IMU data to serial port. It sends the following to serial:
View attachment MPU9250_FiltersGPS_V10.zip
UTC (seconds of day), dt, fix, LLA values, velNED values, PRY values, accelNED values
I'm using this sketch to collect data for testing a simple linear KF. Here's a PDF explaining the simple linear INS/GPS KF:
View attachment SimpleKF, 021018.pdf
Time to collect data... then I'll pull into Matlab and see how the simple KF works. I expect it to perform pretty sloppily, since, the IMU accel measurements will likely only be within 0.01s to the GPS at best, and I haven't done any smoothing yet of those accel values.
Comments/ideas welcome!
EDIT: The MARG 3 filter I'm using takes a few seconds to settle down, so if you use MARG 3 be sure to let it settle.
View attachment MPU9250_FiltersGPS_V10.zip
UTC (seconds of day), dt, fix, LLA values, velNED values, PRY values, accelNED values
I'm using this sketch to collect data for testing a simple linear KF. Here's a PDF explaining the simple linear INS/GPS KF:
View attachment SimpleKF, 021018.pdf
Time to collect data... then I'll pull into Matlab and see how the simple KF works. I expect it to perform pretty sloppily, since, the IMU accel measurements will likely only be within 0.01s to the GPS at best, and I haven't done any smoothing yet of those accel values.
Comments/ideas welcome!
EDIT: The MARG 3 filter I'm using takes a few seconds to settle down, so if you use MARG 3 be sure to let it settle.
Last edited:
defragster
Senior Member+
Changed T_3.6 speed to 180 MHz - setSrd() still not seeing success, when it does work - so recorded _srd is not updated. Didn't trap error value/location yet.
Looking at the GPS nano field again - Indeed very consistent differences { either 199999927, 200000073 or 200000074 }. But that is absolute to PPS when calc started - which we don't have - so not relative to any sense of Teensy/IMU time? Yes it is seen to fire like clockwork as expected - but when was that?
Looking at differences from successive updates of GPS data, there is more variation in the Rx receive times than expected, but that is WHEN the calc was completed and the data started arriving at Teensy Serial#.
May have an implementation detail wrong as ported over using RTC .vs. GPS.PPS or the Rx_isr() trigger, but arrival start time extreme differences seem extreme - near 400 microseconds.
As noted before the value of the Rx interrupt may be just to know when to start buffering/queueing the IMU data reports until the data on the way from the GPS is received to keep it in sync? Not only does GPS calc time impact when it sends - but data sending takes 2+ milliseconds at 460 Kbps - which could be 1 or more IMU readings.
More work to see this is right.
Looking at the GPS nano field again - Indeed very consistent differences { either 199999927, 200000073 or 200000074 }. But that is absolute to PPS when calc started - which we don't have - so not relative to any sense of Teensy/IMU time? Yes it is seen to fire like clockwork as expected - but when was that?
Looking at differences from successive updates of GPS data, there is more variation in the Rx receive times than expected, but that is WHEN the calc was completed and the data started arriving at Teensy Serial#.
May have an implementation detail wrong as ported over using RTC .vs. GPS.PPS or the Rx_isr() trigger, but arrival start time extreme differences seem extreme - near 400 microseconds.
As noted before the value of the Rx interrupt may be just to know when to start buffering/queueing the IMU data reports until the data on the way from the GPS is received to keep it in sync? Not only does GPS calc time impact when it sends - but data sending takes 2+ milliseconds at 460 Kbps - which could be 1 or more IMU readings.
More work to see this is right.
defragster
Senior Member+
Check the INT pin assignment?
mjs513
Senior Member+
Hi Defragster. Did and changed the int pin and gps port (actually put it back to GPS_Start) which is 1. Debugged to point that I am getting IMU data but no GPS data - utcTime and lat are always zero. Also test with the Ublox example sketch and my old GPS threaded sketch which uses your GPS_isr routine - both work. So I am at a loss. I can not see where it shouldn't work. Think I had a few many things to debug today to get working that my brain is mush right now.
UPDATE: Ok was something stupid. Thought I caught all the changes from using hardcoded values instead of the defines. Well I missed one the GPSBaud. Now it works. Driving me nuts and just registered.
UPDATE: Ok was something stupid. Thought I caught all the changes from using hardcoded values instead of the defines. Well I missed one the GPSBaud. Now it works. Driving me nuts and just registered.
Last edited:
Got the Kalman running in Matlab with the collected GPS and IMU data. I'm running the filter in ECEF coordinates, so GPS LLA, GPS velNED, and accNED are all converted to ECEF then processed in the filter. The six filter states are posn xyz and vel xyz in ECEF.
If anyone wants to take a look at the Matlab code, it's here:
https://www.dropbox.com/sh/5k5tgckec3cspuc/AACwuDD2ZvuUB2neLY7E1Sqza?dl=0
Edit: the kfTest.m is the main routine.
It's hard to tell what effect the IMU accel values are having on the filter, primarily because I'm getting both GPS and IMU data at only 5Hz, plus the timetags on the IMU data I suspect are not accurate enough to help in the solution, and may actually be hurting.
I should be able to get the Matlab code ported over to arduino code in the next couple of days.
Edit: Looking at the accel data, it's very very noisy, so suspect we'll need extensive smoothing in addition to accurate timestamp.
If anyone wants to take a look at the Matlab code, it's here:
https://www.dropbox.com/sh/5k5tgckec3cspuc/AACwuDD2ZvuUB2neLY7E1Sqza?dl=0
Edit: the kfTest.m is the main routine.
It's hard to tell what effect the IMU accel values are having on the filter, primarily because I'm getting both GPS and IMU data at only 5Hz, plus the timetags on the IMU data I suspect are not accurate enough to help in the solution, and may actually be hurting.
I should be able to get the Matlab code ported over to arduino code in the next couple of days.
Edit: Looking at the accel data, it's very very noisy, so suspect we'll need extensive smoothing in addition to accurate timestamp.
Last edited:
mjs513
Senior Member+
Don. Will have to take a look at the scripts tomorrow. Spent the day in getting fiterv10 converted to a threaded version (GPS thread and IMU thread). Finally got it working the way I wanted (at least to a point). What was occurring was that the IMU was updating substantially faster than the GPS (expected) but when you tried to use the combined gps/imu data ready tests for printing the GPS data wasn't updating. So what I wound up doing was doing a serialPortOut for gps update which included the latest IMU data then another serialport out for updated imu data (for this lat/lon shows as 0 since no gps data is available). I also did a timestamp (micros to millis zeroed on GPS print and then it updates for each imu update). Can it be better yes. If you want to give it a try I am attaching the zip. Oh, I changed the update rate for the m8n to 100hz.
EDIT. I forgot I add a serial passthrough so you could access ucenter from the teensy but it doesn't really work correctly. Yes it streams to ucenter no problem but ucenter can't read the gps settings and you can't update them. Haven't figured out this one yet.
EDIT. I forgot I add a serial passthrough so you could access ucenter from the teensy but it doesn't really work correctly. Yes it streams to ucenter no problem but ucenter can't read the gps settings and you can't update them. Haven't figured out this one yet.
Last edited:
Mike,
I'll take a look at your zip file Mon. Sounds interesting!
I'll port the Matlab test code over to the arduino Mon or Tue. After looking more at the accel data, and how noisy it is, I'm thinking there are several things I'll need to think about
1) accurate timing of the accel data with respect to UTC time
2) getting faster IMU rates (like the 100Hz you mentioned)
3) smoothing the accel data prior to using it in the Kalman
4) reducing the accel sensitivity to a smaller setting for now (like +-2G or so)
Looking at the Matlab results, I'm encouraged that the simple 6-state filter propagates and updates well, seeming to give good states with low residuals. And even tho the accel is not really helping the solution right now, I can tell it's measuring accel in the right directions when I test movement in N, E, and D directions, so I think that's encouraging too. So it's likely the high sensitivity, noise, and timing I'm thinking...
I'll take a look at your zip file Mon. Sounds interesting!
I'll port the Matlab test code over to the arduino Mon or Tue. After looking more at the accel data, and how noisy it is, I'm thinking there are several things I'll need to think about
1) accurate timing of the accel data with respect to UTC time
2) getting faster IMU rates (like the 100Hz you mentioned)
3) smoothing the accel data prior to using it in the Kalman
4) reducing the accel sensitivity to a smaller setting for now (like +-2G or so)
Looking at the Matlab results, I'm encouraged that the simple 6-state filter propagates and updates well, seeming to give good states with low residuals. And even tho the accel is not really helping the solution right now, I can tell it's measuring accel in the right directions when I test movement in N, E, and D directions, so I think that's encouraging too. So it's likely the high sensitivity, noise, and timing I'm thinking...
mjs513
Senior Member+
Hi Don,
Glad things are looking good with your filter, can't wait to see what you come up with. Your going to make me finish the rc car test bed I was putting together for rtk and waypoint nav testing to do this with now.
Think you are probably right with the things you listed. Before this I was always using 2g and 250dps, but I think that is going to depend you your application. Filtering accel data is always a good thing, I believe that ardupilot even uses a filter with the gyro data. By the way if you change your ranges think about changing the ranges in the GPS nav configuration menu as well.
One of the reasons I wanted to see if I could get gps and imu working with teensythreads was to have each running at its own speed (basically what we set it at - of course there is still some overhead). I haven't done any testing and not sure how so I would leave that to you and defragster
, yes I am lazy. The one question I have is how accurate to you need the timing to be between the imu data and the gps data? Reason I am asking is that if you are planning on doing acrobatics or any crazy maneuvers you may probably loose your GPS for short periods of time or may become unreliable.
Ok, done for tonight I think.
Glad things are looking good with your filter, can't wait to see what you come up with. Your going to make me finish the rc car test bed I was putting together for rtk and waypoint nav testing to do this with now.
Think you are probably right with the things you listed. Before this I was always using 2g and 250dps, but I think that is going to depend you your application. Filtering accel data is always a good thing, I believe that ardupilot even uses a filter with the gyro data. By the way if you change your ranges think about changing the ranges in the GPS nav configuration menu as well.
One of the reasons I wanted to see if I could get gps and imu working with teensythreads was to have each running at its own speed (basically what we set it at - of course there is still some overhead). I haven't done any testing and not sure how so I would leave that to you and defragster
, yes I am lazy. The one question I have is how accurate to you need the timing to be between the imu data and the gps data? Reason I am asking is that if you are planning on doing acrobatics or any crazy maneuvers you may probably loose your GPS for short periods of time or may become unreliable. Ok, done for tonight I think.
defragster
Senior Member+
I keep starting this last few days and then as I type think of code to edit - then not getting back with a good answer to post . . . The last diversion had me wonder how long the IMU processing is taking - and if the case of "if (newData == 1)" on the imu - is that when I see anomalies on my GPS Rx timing :: Answer looks to be YES! WIP ... more below . . .
Brian: Finally put a global for debug tracking on error for requested 'srd' of 49 or 199 in ...\libraries\MPU9250\MPU9250.cpp :: setSrd():
This is after this returns without error:
At a glance I don't see an obvious problem/cause or solution to this? I added a delay(100) [ and 400 or 700 ] before that to no effect as was done on other calls. In the end the srd change is effective - but this intermediate error prevents recognition of that. Is there a limit on the requested SRD? Or reason these calls should fail? During calibration srd=19 completes with no problem but 49 and 199 fail based on the error detected - though the IMU changes. Note: These were found in that the early out on error breaks my effort to fix calibration value reset as stored in the class variables. I am running at 180 MHz so should not be a IMU issues with F_BUS - though I did just bump that to 90 MHz @ 180 and still error -2.
@mjs - yeah we should make a yourNAMEhere_HAL.H file or something for each of us outside the project to #ifdef exclude the local #ifdef's. Glad the #ifdef's got made at the top - I had to add some lines for my ALT pin i2c ... now Don has gone and rev'd the code - while I got distracted on the MPU9250 code and LIFE.
Don: I'll merge my changes into posted V10 filter code - I see changes to main sketch, globals.h, and the serialPort.ino and see if I can't finally get that worth posting. Wondering how UTC time helps - and if it could supersede value of regular local clocking? Since all samples are taken locally and time relative to each other. Other than setting the Teensy RTC now and then. The uBlox nano value only shows the samples were taken - before the calculations - relative to an arbitrary Earth second - before a lengthy 2.2 ms data transfer.
Finding some time offset anomalies from detection of the Rx isr() to the end of the message being 2.8ms instead of 2.2 ms. Not sure if this is stuttering on the uBlox - or isr() detect, some short runs show 1% discrepancy - other longer runs show it 0.4%. Perhaps this happens when IMU data is calculating and the GPD receive is delayed? It seems to happen in small groups of ~5 times in ~10 GPS messages.
mjs: Given the new info 'open to this post' I can see how TThreads could add value to this! And it should be simple to make multiple threads pull their own data - and submit a TIMED RECORD to a queue. Then other processing thread(s) can pull the data RECORDS and process them. In this case the GPS isr() could submit a PENDING RECORD that could HALT imu processing received after that until the GPS complete RECORD is received.
On the current run of 1750 seconds and 8750 GPS messages I have detected 37 GPS messages completing too long after the Start Bit was detected, and they can happen in groups of ~6.
It seems an IMU Calc can take 659 to 780+ micros on the default Filter_V9 code running at 180 MHz. My Rx timing aberrations happen when they are on the longer side (754+ micros). With GPS messages on 5 Hz but stretching 2.2 ms - if the faster Hz imu update cycle overlaps the start to end time gets out of wack and that is what I was detecting and being compulsive about - when a 2.2ms xfer takes 2.8ms. I correctly only ever trigger one isr() per GPS msg, but sometimes the finish was too far away to seem kosher so it seemed there was a Start bit detection error on the isr()!
TThreads would jump from arbitrarily long imu/filter updates harmlessly - ping the GPS Serial and then yield - perhaps with completed data record. For FIFO xfer and uBlox message read/convert uses probably net 1ms per second of CPU time - but not getting the messages in a timely fashion is uncalled for.
Brian: Finally put a global for debug tracking on error for requested 'srd' of 49 or 199 in ...\libraries\MPU9250\MPU9250.cpp :: setSrd():
Code:
if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){
return ErrSS=-2;
}
Code:
/* setting the sample rate divider */
if(writeRegister(SMPDIV,srd) < 0){ // setting the sample rate dividerAt a glance I don't see an obvious problem/cause or solution to this? I added a delay(100) [ and 400 or 700 ] before that to no effect as was done on other calls. In the end the srd change is effective - but this intermediate error prevents recognition of that. Is there a limit on the requested SRD? Or reason these calls should fail? During calibration srd=19 completes with no problem but 49 and 199 fail based on the error detected - though the IMU changes. Note: These were found in that the early out on error breaks my effort to fix calibration value reset as stored in the class variables. I am running at 180 MHz so should not be a IMU issues with F_BUS - though I did just bump that to 90 MHz @ 180 and still error -2.
@mjs - yeah we should make a yourNAMEhere_HAL.H file or something for each of us outside the project to #ifdef exclude the local #ifdef's. Glad the #ifdef's got made at the top - I had to add some lines for my ALT pin i2c ... now Don has gone and rev'd the code - while I got distracted on the MPU9250 code and LIFE.
Don: I'll merge my changes into posted V10 filter code - I see changes to main sketch, globals.h, and the serialPort.ino and see if I can't finally get that worth posting. Wondering how UTC time helps - and if it could supersede value of regular local clocking? Since all samples are taken locally and time relative to each other. Other than setting the Teensy RTC now and then. The uBlox nano value only shows the samples were taken - before the calculations - relative to an arbitrary Earth second - before a lengthy 2.2 ms data transfer.
Finding some time offset anomalies from detection of the Rx isr() to the end of the message being 2.8ms instead of 2.2 ms. Not sure if this is stuttering on the uBlox - or isr() detect, some short runs show 1% discrepancy - other longer runs show it 0.4%. Perhaps this happens when IMU data is calculating and the GPD receive is delayed? It seems to happen in small groups of ~5 times in ~10 GPS messages.
mjs: Given the new info 'open to this post' I can see how TThreads could add value to this! And it should be simple to make multiple threads pull their own data - and submit a TIMED RECORD to a queue. Then other processing thread(s) can pull the data RECORDS and process them. In this case the GPS isr() could submit a PENDING RECORD that could HALT imu processing received after that until the GPS complete RECORD is received.
On the current run of 1750 seconds and 8750 GPS messages I have detected 37 GPS messages completing too long after the Start Bit was detected, and they can happen in groups of ~6.
It seems an IMU Calc can take 659 to 780+ micros on the default Filter_V9 code running at 180 MHz. My Rx timing aberrations happen when they are on the longer side (754+ micros). With GPS messages on 5 Hz but stretching 2.2 ms - if the faster Hz imu update cycle overlaps the start to end time gets out of wack and that is what I was detecting and being compulsive about - when a 2.2ms xfer takes 2.8ms. I correctly only ever trigger one isr() per GPS msg, but sometimes the finish was too far away to seem kosher so it seemed there was a Start bit detection error on the isr()!
TThreads would jump from arbitrarily long imu/filter updates harmlessly - ping the GPS Serial and then yield - perhaps with completed data record. For FIFO xfer and uBlox message read/convert uses probably net 1ms per second of CPU time - but not getting the messages in a timely fashion is uncalled for.
mjs513
Senior Member+
defragster.
A lot of info to absorb on no coffee but all great. By the way call me mike don't worry about he mjs.
. BTW did you see this post https://forum.pjrc.com/threads/44723-Arduino-Events?p=153816&viewfull=1#post153816
A lot of info to absorb on no coffee but all great. By the way call me mike don't worry about he mjs.
The #ifdefs grew on me so I started to make I put it back in the sketches. That was the problem I ran into with non-receipt of GPS data. LIFE does always seem to get in the way of fun sometimes doesn't it.
At what point in the thread would you suggest the timestamp be tracked - beginning of thread - end of thread? I may be off here but would eventresponder be of any use here? or ISR just suffice. EDIT: Just saw where you asked this question on the Arduino event thread
. BTW did you see this post https://forum.pjrc.com/threads/44723-Arduino-Events?p=153816&viewfull=1#post153816Agree
Last edited:
Mike,
Just studied your threading code. Really impressive! Wow!!
As i port the ins/gps KF over from matlab, i’ll build it as a separate tab/function so it can be called easily. I’m thinking i’ll build a KFpropagation function that would be called at each new imu posting, and then a KFupdate that would be called at each gps posting.
The KFpropagation could be called slower as well. For example, if we’re getting imu values at 100Hz, we might choose to smooth every 5 and only perform KFpropagate at 20Hz with the smoothed accel and PRY values. So several options probably.
Just studied your threading code. Really impressive! Wow!!
As i port the ins/gps KF over from matlab, i’ll build it as a separate tab/function so it can be called easily. I’m thinking i’ll build a KFpropagation function that would be called at each new imu posting, and then a KFupdate that would be called at each gps posting.
The KFpropagation could be called slower as well. For example, if we’re getting imu values at 100Hz, we might choose to smooth every 5 and only perform KFpropagate at 20Hz with the smoothed accel and PRY values. So several options probably.
mjs513
Senior Member+
Morning Don
Your approach to have separate propagation/update functions for imu/gps makes a lot of sense and should improve the results dramatically.
Not sure about the way most of it was just rearranging some code. The tough part for me was figuring out the reason for the dups in lat/long and what to do about it - that took me all day. But then reading defragster's post it explained what was going on so it makes sense.
Your approach to have separate propagation/update functions for imu/gps makes a lot of sense and should improve the results dramatically.
defragster
Senior Member+
defragster.
A lot of info to absorb on no coffee but all great. By the way call me mike don't worry about he mjs.
The #ifdefs grew on me so I started to make I put it back in the sketches. That was the problem I ran into with non-receipt of GPS data. LIFE does always seem to get in the way of fun sometimes doesn't it.
At what point in the thread would you suggest the timestamp be tracked - beginning of thread - end of thread? I may be off here but would eventresponder be of any use here? or ISR just suffice. EDIT: Just saw where you asked this question on the Arduino event thread
Agree
Mike, It was a lot to write too - hopefully it makes sense and is meaningfully on some point.
It seems the isr() for IMU and that I have for GPS_Rx are good at time stamping - closest known 'time' after the sample was taken. How they get used after that depends on what is done with data management? A FIFO buffer could help with taking the newest data first, and holding off when GPS data is in flight.
Dealing with my PartSec it is accurate and easy - but the time it is based on refreshes with RTC clock ticks each second so getting it with the RTC second tick seems critical - where the PartSec would be the decimal part. With a PS_TimeStruct the values could include ticks.PartSec and the base uint32_t cycle count info for absolute compares good for up to 17 seconds (when a 240 MHz cpu wraps on the count).
TThreads or not it still seems the GPS data for best use would be integrated in order of the IMU data where it affects the outcome and buffering the data could allow that.
About the link to Signals - not sure if that is somehow implemented in Teensy Threads [ I don't see it - except Mutex's ] to support halting/informing an IMU thread that should wait for GPS data arrival.
For Threads I suppose this describes what applies? : Producer–consumer_problem. When I first chatted with Don last Christmas I assumed data would be stored then pulled and processed later - the raw data isn't large so keeping a second of IMU and GPS would be under 3K?
mjs513
Senior Member+
defragster.
As always it is fun reading your posts - they always contain a wealth of information. Think now that I have it working and understand a little more about whats going on I am going to go back and put back in your ISR code unless you updated from what you posted earlier in this thread on TThreads.
You can use polling to get data from the GPS but that leads to other complications, you can poll it at a fixed rate as opposed to waiting for data to be ready and the parse it. Not sure of the delay that adds.
One of the other things for shared memory is the use of atomic variables, which is on my list of things to do and get a full grasp on.
As always it is fun reading your posts - they always contain a wealth of information. Think now that I have it working and understand a little more about whats going on I am going to go back and put back in your ISR code unless you updated from what you posted earlier in this thread on TThreads.
You can use polling to get data from the GPS but that leads to other complications, you can poll it at a fixed rate as opposed to waiting for data to be ready and the parse it. Not sure of the delay that adds.
One of the other things for shared memory is the use of atomic variables, which is on my list of things to do and get a full grasp on.
defragster
Senior Member+
Hopefully more fun reading than writing them - if not that would be painful
Just opened the code I think I shared for TThreads and that in essence hasn't changed ( I knew I had posted some version of it ). Along with the support code to set it up to the right Serial pin for isr().
I modified what I'm using now to use the raw cycle counter rather than micros() or even my PartSec - almost no overhead to tell when mid-message isr() calls result. Working very well - if I show all the parts below. I hacked out my DEBUG vars that I was using to track reliability.
Here is from my current code using micros() as the GPS time marker. And the setup() initTiming function to start the cycle counter - note InitTiming() must be called after initGPS() or it kills the isr(). This should work. Post your code if you try it.
Seeing the IMU Filter code currently eating 750+ micros says that using TeensyThreads might be the right way to go - and it looks robust and simple enough. Note - I saw "void yield()" in my sketch - AFAIK that is NOT needed when using TThreads as it already replaces it.
Code:
#define RxGapTime ((F_CPU / GPS_BAUD) * 200)
// FASTRUN void GPS_serialrx_isr()
[B]// GLOBALS.h add :: volatile uint32_t GPS_RX_Time = 0;[/B]
FASTRUN void GPS_serialrx_isr() {
static uint32_t tt;
static uint32_t xx = 0;
tt = ARM_DWT_CYCCNT;
if ( tt - xx > RxGapTime ) {
GPS_RX_Time = micros();
}
xx = tt;
}
Code:
void initTiming() {
[B] // MUST BE CALLED AFTER initGPS()[/B]
// Enable CPU Cycle Count
ARM_DEMCR |= ARM_DEMCR_TRCENA;
ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA;
attachInterrupt(digitalPinToInterrupt(GPS_SRX), GPS_serialrx_isr, RISING);
}I have no reliable scheme for variable/#define naming - if you don't appreciate anything - please adjust.
defragster
Senior Member+
The GPS code is basically polling - that is the trouble when the Filter update code update takes the time it does.
If that polling is done ( like in prior sketch ) on a thread that wakes - calls gps.read(&uBloxData) then threads.yield(); as before it should take very little time to accumulate bytes to completion and quickly exit when GPS_PORT.available() is zero.
If that polling is done ( like in prior sketch ) on a thread that wakes - calls gps.read(&uBloxData) then threads.yield(); as before it should take very little time to accumulate bytes to completion and quickly exit when GPS_PORT.available() is zero.
- Status