uNav INS

mjs513 · Apr 2, 2018

Yeah, in my haste to get you guys an updated version I missed a couple of additional changes. I really don't know what happened.

defragster · Apr 2, 2018

Mike - trying to save a post - I was editing #225 when you posted - now on page #9 and this is #10 ... what is your Loop# and IMU# with T_3.5?

mjs513 · Apr 2, 2018

"Had to set coutD to Serial " - ok. don't blame me for that one - think I am feeling guilty.

Anyway with an SRD = 9:
-----Loop#:310214 IMU#:102
an SRD = 4:
-----Loop#:271168 IMU#:208

defragster · Apr 2, 2018

Cool - you have some spare CPU time left at a decent IMU update rate - that is at 168 MHz? Would be interesting to see Not TViewerSPI results.

Shows T_3.6 brings a bit more oomph than just clock speed even at 180 MHZ : -----Loop#:531321 IMU#:251.

BTW: updated _V4 doing fine so far at 1700 seconds.

mjs513 · Apr 2, 2018

The real test is at about 4280-4285 when it rolls over.

Don Kelly · Apr 2, 2018

tonton81,

Just tested your circular-buffer example (post #215) with the floats. I verified your results with Matlab. Nice work!!

I tried also setting the buffer to:

Circular_Buffer<float, 4> myFloats;
myFloats.push_back(3.14159);
myFloats.push_back(12.3456);
myFloats.push_back(78.91234);
myFloats.push_back(7.91234);
myFloats.push_back(11.91234);
myFloats.push_back(58.91234);
myFloats.push_back(18.91234);

and sure enough it just gives me the results using the last 4 in the buffer. Slick!!

Testing Floats and Statistics
MEAN: 24.41234
DEVIATION (std): 20.30394
MIN: 7.91234
MAX: 58.91234

So I think I know enough to put the hooks into uNavINS and try a test case with the real data...
Don

tonton81 · Apr 2, 2018

im glad you like it

, that buffer system is in a ballpark of its own hehe

mjs513 · Apr 2, 2018

Hi Tim.... Well did it work?

defragster · Apr 2, 2018

mjs513 said:
Hi Tim.... Well did it work?

I'm up to :: 2983.906738 ... and counting ... not past rollover ... 4300

2983.906738, 51.259735,152.075363,-151.712738, 60.662827, 48.214451,-122.450378, 45.299561, -0.007418, -0.056441, 0.042388, -0.000072, -0.000507, 0.000055, -2.775813, 3.439847, 2.829954, 0.554931, 0.554943, 0.717299, 0.485581, 0.486623, 0.478657, 0.050067, 0.032897, 0.042869, 0.089746, 0.093950, 0.095346, 0.006734, 0.006008, 0.006396, 0.399753, 0.702496, 0.531943, -0.252457,

No apparent problem yet .... -----Loop#:528562 IMU#:250

mjs513 · Apr 2, 2018

Ok, thanks was just curious. This just takes so long - have to try and force it for somehow so don't have to wait so long

tonton81 · Apr 2, 2018

With all due respect, I present to you more kinder surprise for Circular_Buffering

Code:

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

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

You have new methods, and yes, I'm not joking...

Code:

        T variance(); [COLOR="#FF0000"]Yes!, variance![/COLOR]
        T deviation(); [COLOR="#FF0000"]Yes!, standard deviation![/COLOR]
        T average(); [COLOR="#FF0000"]Yes!, averaging!![/COLOR]
        T median(); [COLOR="#FF0000"]Yes!, the MEDIAN! sorts your array in ascending order and provides you the MEDIAN!![/COLOR]
        void sort_ascending(); [COLOR="#FF0000"]Yes!, this sorts your entire buffer in ascending order![/COLOR]
        void sort_descending(); [COLOR="#FF0000"]Yes!, this sorts your entire buffer in descending order![/COLOR]
        T sum();  [COLOR="#FF0000"]Yes!, this sums up all your entries![/COLOR]
        T min();  [COLOR="#FF0000"]Yes!, smallest value in queue![/COLOR]
        T max(); [COLOR="#FF0000"]Yes!, largest value in queue![/COLOR]
        T mean() { return average(); } [COLOR="#FF0000"]Yes!, same as average()![/COLOR]

Output:

Code:

C:\examples\master\master.ino Apr  2 2018 17:46:11
SUM: 192.04887
MIN: 3.14159
MAX: 78.91234
MEDIAN: 12.34560
AVG: 27.43556
Deviation: 27.13280106
3.14159 7.91234 11.91234 12.34560 18.91234 58.91234 78.91234

https://github.com/tonton81/Circular_Buffer/commit/87804f450e3424722639333360746bcad8b11273

Don Kelly · Apr 2, 2018

Mike,
Just got your latest uNavINS up and running. I seem to have to set IMU_SPD to 1000000 to get it to run. I need to read up on that and see if I can figure out why.

Don

defragster · Apr 2, 2018

Don Kelly said:
Mike,
Just got your latest uNavINS up and running. I seem to have to set IMU_SPD to 1000000 to get it to run. I need to read up on that and see if I can figure out why.

Don

Don - a T_3.6 at 180 ? Mine takes 2,000,000 - the onehorse soldered under the Teensy - didn't push for anything above when that worked. And at 240 as noted I can get to 3.4 MHz.

mjs513 said:
Ok, thanks was just curious. This just takes so long - have to try and force it for somehow so don't have to wait so long

4251 and counting at least it isn't 86,400. .... getting close ...

tonton81 - very cool - maybe if you go through each supported method and write up a DOC on use for github ... you'll find the next two things you NEED to add

defragster · Apr 2, 2018

Mikie: This looks bad??? integer part gone NEGATIVE

Code:

4281.767090, 51.150784,152.014343,-151.703049, 60.672504, 48.214344,-122.450325, 50.748638,  0.024716,  0.022847, -0.011895,  0.000388, -0.000671,  0.000228, -2.775480,  3.437612,  2.831618,  0.566353,  0.566402,  0.726079,  0.586638,  0.588414,  0.574422,  0.050243,  0.033062,  0.042984,  0.089755,  0.093965,  0.095385,  0.006754,  0.006029,  0.006410,  0.399813,  0.702687,  0.532159, -0.251372,
4281.795410, 51.154629,151.991196,-151.739410, 60.636162, 48.214344,-122.450325, 50.704826,  0.038776,  0.014108, -0.019452,  0.000394, -0.000659,  0.000209, -2.775562,  3.437585,  2.831589,  0.549467,  0.549470,  0.712978,  0.400167,  0.400752,  0.396359,  0.049902,  0.032756,  0.042661,  0.089717,  0.093943,  0.095369,  0.006726,  0.005992,  0.006379,  0.399888,  0.702759,  0.531986, -0.251418,
4281.823242, 51.154144,151.990082,-151.740921, 60.634651, 48.214344,-122.450325, 50.705376,  0.039208,  0.014497, -0.019746,  0.000394, -0.000659,  0.000209, -2.775562,  3.437585,  2.831589,  0.551394,  0.551399,  0.714490,  0.435224,  0.435972,  0.430273,  0.049957,  0.032806,  0.042714,  0.089723,  0.093947,  0.095372,  0.006730,  0.005998,  0.006384,  0.399890,  0.702764,  0.531979, -0.251415,
4281.851074, 51.153294,151.988235,-151.742966, 60.632595, 48.214344,-122.450325, 50.705925,  0.039755,  0.014900, -0.019667,  0.000394, -0.000659,  0.000209, -2.775562,  3.437585,  2.831589,  0.553583,  0.553593,  0.716201,  0.467898,  0.468822,  0.461712,  0.050013,  0.032856,  0.042766,  0.089729,  0.093950,  0.095374,  0.006735,  0.006004,  0.006389,  0.399894,  0.702771,  0.531971, -0.251407,
4281.878906, 51.152008,151.988312,-151.743347, 60.632221, 48.214344,-122.450325, 50.706470,  0.040197,  0.015342, -0.019483,  0.000394, -0.000659,  0.000209, -2.775562,  3.437585,  2.831589,  0.556076,  0.556091,  0.718141,  0.498664,  0.499774,  0.491155,  0.050068,  0.032906,  0.042819,  0.089736,  0.093954,  0.095377,  0.006739,  0.006010,  0.006394,  0.399889,  0.702778,  0.531969, -0.251400,
-13.060497, 51.151535,151.987900,-151.743134, 60.632439, 48.214344,-122.450325, 50.707016,  0.040727,  0.015853, -0.019418,  0.000394, -0.000659,  0.000209, -2.775562,  3.437585,  2.831589,  0.558910,  0.558933,  0.720339,  0.527858,  0.529166,  0.518943,  0.050124,  0.032956,  0.042872,  0.089742,  0.093957,  0.095380,  0.006744,  0.006016,  0.006399,  0.399890,  0.702777,  0.531971, -0.251394,
-13.032531, 51.150871,151.988663,-151.742310, 60.633259, 48.214344,-122.450325, 50.707558,  0.041492,  0.016337, -0.019280,  0.000394, -0.000659,  0.000209, -2.775562,  3.437585,  2.831589,  0.562123,  0.562156,  0.722822,  0.555727,  0.557245,  0.545325,  0.050180,  0.033006,  0.042925,  0.089748,  0.093961,  0.095382,  0.006749,  0.006022,  0.006404,  0.399885,  0.702779,  0.531974, -0.251390,
-13.004562, 51.150284,151.988739,-151.742325, 60.633240, 48.214344,-122.450325, 50.708096,  0.042203,  0.016846, -0.019148,  0.000394, -0.000659,  0.000209, -2.775562,  3.437585,  2.831589,  0.565753,  0.565800,  0.725618,  0.582465,  0.584203,  0.570500,  0.050236,  0.033056,  0.042978,  0.089754,  0.093964,  0.095385,  0.006753,  0.006028,  0.006410,  0.399883,  0.702782,  0.531974, -0.251386,
-12.976594, 51.169140,152.028091,-151.703598, 60.671967, 48.214344,-122.450325, 50.661301,  0.001158,  0.009430, -0.008960,  0.000373, -0.000647,  0.000252, -2.775460,  3.437671,  2.831590,  0.549201,  0.549204,  0.712768,  0.394817,  0.395379,  0.391167,  0.049889,  0.032750,  0.042653,  0.089716,  0.093943,  0.095369,  0.006725,  0.005991,  0.006378,  0.399784,  0.702662,  0.532121, -0.251567,
-12.948627, 51.168716,152.026993,-151.705658, 60.669926, 48.214344,-122.450325, 50.661545,  0.001394,  0.009911, -0.008618,  0.000373, -0.000647,  0.000252, -2.775460,  3.437671,  2.831590,  0.551090,  0.551095,  0.714251,  0.430268,  0.430989,  0.425491,  0.049945,  0.032799,  0.042706,  0.089722,  0.093946,  0.095371,  0.006730,  0.005997,  0.006383,  0.399785,  0.702669,  0.532111, -0.251566,
-12.920656, 51.166653,152.027435,-151.705612, 60.669956, 48.214344,-122.450325, 50.661785,  0.002041,  0.010334, -0.008576,  0.000373, -0.000647,  0.000252, -2.775460,  3.437671,  2.831590,  0.553237,  0.553246,  0.715930,  0.463260,  0.464157,  0.457262,  0.050000,  0.032849,  0.042759,  0.089728,  0.093950,  0.095374,  0.006734,  0.006003,  0.006388,  0.399776,  0.702679,  0.532111, -0.251553,
-12.892690, 51.166237,152.028076,-151.705399, 60.670155, 48.214344,-122.450325, 50.662025,  0.002637,  0.010648, -0.008538,  0.000373, -0.000647,  0.000252, -2.775460,  3.437671,  2.831590,  0.555681,  0.555695,  0.717833,  0.494278,  0.495363,  0.486970,  0.050056,  0.032899,  0.042812,  0.089735,  0.093953,  0.095376,  0.006739,  0.006009,  0.006393,  0.399772,  0.702682,  0.532111, -0.251553,
-12.864721, 51.166859,152.030136,-151.704865, 60.670715, 48.214344,-122.450325, 50.662266,  0.003402,  0.010498, -0.008784,  0.000373, -0.000647,  0.000252, -2.775460,  3.437671,  2.831590,  0.558461,  0.558482,  0.719990,  0.523684,  0.524972,  0.514982,  0.050113,  0.032947,  0.042865,  0.089741,  0.093957,  0.095379,  0.006743,  0.006015,  0.006399,  0.399762,  0.702684,  0.532110, -0.251565,

It turns around here - no NaN's but ugly?

Code:

 -0.175187, 51.234913,152.163895,-151.552826, 60.822742, 48.214344,-122.450340, 48.339622, -0.011078, -0.027151,  0.025878,  0.000277, -0.000562,  0.000410, -2.775836,  3.437840,  2.831308,  0.549347,  0.549350,  0.712893,  0.394928,  0.395488,  0.391275,  0.049920,  0.032745,  0.042643,  0.089716,  0.093944,  0.095368,  0.006724,  0.005992,  0.006379,  0.399486,  0.702164,  0.532733, -0.252136,
 -0.147220, 51.233383,152.162384,-151.554733, 60.820835, 48.214344,-122.450340, 48.338898, -0.011199, -0.026613,  0.025893,  0.000277, -0.000562,  0.000410, -2.775836,  3.437840,  2.831308,  0.551239,  0.551244,  0.714379,  0.430371,  0.431091,  0.425589,  0.049975,  0.032795,  0.042696,  0.089722,  0.093947,  0.095370,  0.006728,  0.005998,  0.006384,  0.399486,  0.702174,  0.532725, -0.252124,
 -0.119251, 51.233734,152.160767,-151.556091, 60.819466, 48.214344,-122.450340, 48.338173, -0.011156, -0.026472,  0.025835,  0.000277, -0.000562,  0.000410, -2.775836,  3.437840,  2.831308,  0.553388,  0.553397,  0.716059,  0.463355,  0.464250,  0.457351,  0.050032,  0.032845,  0.042747,  0.089728,  0.093951,  0.095373,  0.006733,  0.006004,  0.006389,  0.399494,  0.702173,  0.532720, -0.252124,
 -0.091282, 51.234203,152.161179,-151.555878, 60.819691, 48.214344,-122.450340, 48.337452, -0.011155, -0.026227,  0.025886,  0.000277, -0.000562,  0.000410, -2.775836,  3.437840,  2.831308,  0.555835,  0.555849,  0.717965,  0.494370,  0.495452,  0.487057,  0.050088,  0.032895,  0.042799,  0.089734,  0.093954,  0.095376,  0.006737,  0.006010,  0.006394,  0.399493,  0.702172,  0.532721, -0.252129,
 -0.063315, 51.233459,152.162048,-151.556137, 60.819431, 48.214344,-122.450340, 48.336731, -0.011109, -0.026123,  0.025536,  0.000277, -0.000562,  0.000410, -2.775836,  3.437840,  2.831308,  0.558617,  0.558639,  0.720124,  0.523774,  0.525056,  0.515065,  0.050144,  0.032944,  0.042853,  0.089740,  0.093958,  0.095378,  0.006742,  0.006016,  0.006399,  0.399485,  0.702179,  0.532717, -0.252128,
 -0.035346, 51.231747,152.161331,-151.558044, 60.817539, 48.214344,-122.450340, 48.336021, -0.011440, -0.025865,  0.025363,  0.000277, -0.000562,  0.000410, -2.775836,  3.437840,  2.831308,  0.561774,  0.561805,  0.722565,  0.551823,  0.553311,  0.541637,  0.050199,  0.032995,  0.042906,  0.089747,  0.093961,  0.095381,  0.006746,  0.006021,  0.006404,  0.399480,  0.702192,  0.532708, -0.252119,
 -0.007379, 51.231190,152.158005,-151.561096, 60.814472, 48.214344,-122.450340, 48.335312, -0.011374, -0.025801,  0.025155,  0.000277, -0.000562,  0.000410, -2.775836,  3.437840,  2.831308,  0.565341,  0.565386,  0.725315,  0.578713,  0.580420,  0.566974,  0.050255,  0.033046,  0.042958,  0.089753,  0.093965,  0.095384,  0.006751,  0.006027,  0.006410,  0.399492,  0.702197,  0.532696, -0.252111,
  0.016594, 51.192352,152.189545,-151.493423, 60.882153, 48.214344,-122.450340, 48.324699, -0.003900,  0.021498,  0.014270,  0.000276, -0.000633,  0.000414, -2.775946,  3.437802,  2.831292,  0.548831,  0.548833,  0.712488,  0.384143,  0.384662,  0.380796,  0.049901,  0.032734,  0.042630,  0.089714,  0.093943,  0.095367,  0.006722,  0.005990,  0.006377,  0.399328,  0.702152,  0.533050, -0.251748,
  0.044562, 51.193150,152.188278,-151.493546, 60.882023, 48.214344,-122.450340, 48.324299, -0.004125,  0.021725,  0.014326,  0.000276, -0.000633,  0.000414, -2.775946,  3.437802,  2.831292,  0.550653,  0.550658,  0.713921,  0.420418,  0.421094,  0.415973,  0.049957,  0.032783,  0.042682,  0.089720,  0.093946,  0.095370,  0.006727,  0.005996,  0.006382,  0.399338,  0.702145,  0.533052, -0.251748,
  0.072530, 51.193974,152.186310,-151.495834, 60.879742, 48.214344,-122.450340, 48.323898, -0.004275,  0.022009,  0.014161,  0.000276, -0.000633,  0.000414, -2.775946,  3.437802,  2.831292,  0.552722,  0.552730,  0.715541,  0.454054,  0.454900,  0.448413,  0.050014,  0.032833,  0.042733,  0.089726,  0.093950,  0.095372,  0.006731,  0.006002,  0.006388,  0.399347,  0.702146,  0.533042, -0.251753,
  0.100497, 51.195335,152.184265,-151.498413, 60.877159, 48.214344,-122.450340, 48.323505, -0.004056,  0.022439,  0.014052,  0.000276, -0.000633,  0.000414, -2.775946,  3.437802,  2.831292,  0.555077,  0.555089,  0.717377,  0.485598,  0.486630,  0.478671,  0.050071,  0.032884,  0.042784,  0.089733,  0.093954,  0.095375,  0.006736,  0.006008,  0.006393,  0.399358,  0.702145,  0.533031, -0.251762,
  0.128466, 51.196114,152.182510,-151.501022, 60.874550, 48.214344,-122.450340, 48.323112, -0.003971,  0.022694,  0.013868,  0.000276, -0.000633,  0.000414, -2.775946,  3.437802,  2.831292,  0.557756,  0.557775,  0.719458,  0.515433,  0.516662,  0.507135,  0.050128,  0.032934,  0.042835,  0.089739,  0.093957,  0.095378,  0.006740,  0.006014,  0.006398,  0.399366,  0.702147,  0.533018, -0.251768,
  0.156435, 51.196213,152.182312,-151.502548, 60.873013, 48.214344,-122.450340, 48.322727, -0.004222,  0.022976,  0.013869,  0.000276, -0.000633,  0.000414, -2.775946,  3.437802,  2.831292,  0.560799,  0.560827,  0.721813,  0.543846,  0.545279,  0.534097,  0.050184,  0.032984,  0.042888,  0.089745,  0.093960,  0.095380,  0.006745,  0.006020,  0.006403,  0.399364,  0.702154,  0.533009, -0.251773,
  0.184403, 51.196678,152.182159,-151.503632, 60.871941, 48.214344,-122.450340, 48.322338, -0.004423,  0.023182,  0.013813,  0.000276, -0.000633,  0.000414, -2.775946,  3.437802,  2.831292,  0.564242,  0.564282,  0.724469,  0.571051,  0.572698,  0.559772,  0.050241,  0.033033,  0.042940,  0.089751,  0.093964,  0.095383,  0.006749,  0.006026,  0.006408,  0.399364,  0.702156,  0.533003, -0.251780,

Don Kelly · Apr 2, 2018

ut oh, time went negative.... we've transported into a new dimension... the Teensy Twilight Zone...

tonton81 · Apr 2, 2018

Tim, thanks, but I hate paperwork, my eyes are too tired and I always make typo mistakes.

But, thanks for the hint, I patched median() with a bool overload:

Code:

  Serial.print("MEDIAN: "); Serial.println(myFloats.median(), 5);[COLOR="#FF0000"]does not sort the internal buffer but does return the median[/COLOR]
  Serial.print("MEDIAN: "); Serial.println(myFloats.median(1), 5);[COLOR="#FF0000"]overrides the internal buffer in ascending order[/COLOR]

This way you can get the median without modifying the queue entries, unless you specify with bool, or directly call sort_ascending/sort_descending

https://github.com/tonton81/Circular_Buffer/commit/c54bbf82bafaddc491e646e7a22bd469c8a20dfe

I would say babysteps, maybe I'll figure out what to write by this weekend, nobody else must be using it with thousands of views and no feedback, like all the other stuff i pushed out

lack of motivation is tiring hehe

defragster · Apr 2, 2018

Don:
We seem to have survived the time Cusp - I don't feel different:

Code:

499.990997, 51.269238,152.047424,-151.788727, 60.586826, 48.214378,-122.450371, 43.788082, -0.009112, -0.016472, -0.025780,  0.000169, -0.000588,  0.000226, -2.775545,  3.438407,  2.831053,  0.566226,  0.566275,  0.725972,  0.586100,  0.587876,  0.573921,  0.050297,  0.033095,  0.043011,  0.089759,  0.093964,  0.095394,  0.006753,  0.006030,  0.006411,  0.399786,  0.702712,  0.531536, -0.252659,
500.018951, 51.248112,152.046524,-151.783630, 60.591938, 48.214378,-122.450371, 43.813961,  0.011710,  0.006698, -0.028033,  0.000172, -0.000620,  0.000209, -2.775614,  3.438306,  2.831083,  0.549399,  0.549402,  0.712914,  0.400035,  0.400620,  0.396233,  0.049963,  0.032790,  0.042696,  0.089722,  0.093943,  0.095378,  0.006726,  0.005993,  0.006380,  0.399738,  0.702772,  0.531574, -0.252488,

// ...

734.167847, 51.283283,152.057510,-151.727753, 60.647808, 48.214401,-122.450325, 51.663200,  0.033758, -0.009564, -0.075562,  0.000310, -0.000719,  0.000302, -2.774274,  3.437416,  2.832337,  0.563206,  0.563243,  0.723670,  0.563383,  0.564971,  0.552552,  0.050229,  0.033042,  0.042943,  0.089752,  0.093962,  0.095391,  0.006748,  0.006024,  0.006408,  0.399901,  0.702406,  0.531885, -0.252593,
734.195801, 51.283829,152.060043,-151.726746, 60.648830, 48.214401,-122.450325, 51.665310,  0.033820, -0.009607, -0.075540,  0.000310, -0.000719,  0.000302, -2.774274,  3.437416,  2.832337,  0.566958,  0.567009,  0.726558,  0.589832,  0.591643,  0.577417,  0.050286,  0.033091,  0.042996,  0.089758,  0.093965,  0.095394,  0.006753,  0.006030,  0.006413,  0.399890,  0.702408,  0.531886, -0.252605,

tonton81 said:
Tim, thanks, but I hate paperwork, my eyes are too tired and I always make typo mistakes.

But, thanks for the hint, I patched median() with a bool overload:

Code:

Serial.print("MEDIAN: "); Serial.println(myFloats.median(), 5);[COLOR="#FF0000"]does not sort the internal buffer but does return the median[/COLOR] Serial.print("MEDIAN: "); Serial.println(myFloats.median(1), 5);[COLOR="#FF0000"]overrides the internal buffer in ascending order[/COLOR]

This way you can get the median without modifying the queue entries, unless you specify with bool, or directly call sort_ascending/sort_descending

https://github.com/tonton81/Circular_Buffer/commit/c54bbf82bafaddc491e646e7a22bd469c8a20dfe

Dang - it will be a lot more useful with good docs - we all hate writing and paperwork I assume

Even reading the code isn't ... normal code ... I assume !multi means it is a CB not a CA . . .

I'll proofread them if you make a first pass ....

Don Kelly · Apr 2, 2018

Tim,
Just tried IMU_SPD 2000000 again, no luck with the SparkFun IMU9250. But IMU_SPD 1000000 seems to work just fine. Weird.

Switching over to see if I can incorporate Circular_buffer calculations just prior to the serial prints to SPI. Am going to start VERY simple with ~10 terms, including just one state and one covariance. Should be interesting!!

Don

tonton81 · Apr 2, 2018

yeah multi belongs to the array system, we block access as theres nothing useful for it there

but im sure today's features is wanted by many users, would be a shame to throw this library on the back burner and lost in github space lol

Tim, do not forget, the array system is smart, just because you have a length of 200 for the array sizes, doesnt mean it's full. When a user writes something to the array, the length value is stored within the first 2 positions of the internal system array, and your raw array size is not what you think (200), it's actually 202. The library internally modifies the buffer to give the user his 200 space but reserves the first 2 of the 202 for the length;

All your array queues have known lengths to the library, if I wanted to add TODAY's features, I could, library would pull the dataset by length (not the full raw), so it's sorting/median/deviation, whatever today you throw at it, and it'll work

Essentially, if you wanted to push_back the array to your queue, and do a sort_back_ascending(), it would put all your contents in ascending order

mjs513 · Apr 2, 2018

Ok. Just finished diner and got back to this.

Tim- that makes no sense, when I ran the test case, it rolled over to a +10.xxx, for the T3.5. EDIT: It might. I printing every 10ms so I may have missed it.

To shorten the test time couldn't we just set _t = 4280 after initialization? EDIT: NOPE, tried it.

Don. I added this code to the config tab:

Code:

  #if (F_CPU == 240000000)
    #define IMU_SPD   3400000  //1000000 // 0==NULL or other
  #else
    #define IMU_SPD   2000000  //1000000 // 0==NULL or other
  #endif

Like Tim said - I'm running at the T3.5 at 2000000 without a problem at 168Mhz. I was having problems at 400khz though. EDIT: Maybe it size of the pullups Sparkfun uses?

EDIT: Ok figure out why we survived the cusp. Yes dt was going negative so it drove the NaNs which cause us grief. That was the main issue.

rtcTime which is the first column of the output isn't directly related to dt except in a round about way (they both used mircos in the same loop). In the loop it is defined as:

Code:

  rtcTime = (float) micros() / 1000000.0 - rtcOffset;

but rtcTime is not used for calculations in the KF. rtcTime can still go negative with out affecting the filter, i.e., giving us NaNs. So this needs a better way to track time. Just not sure the best approach.

defragster · Apr 2, 2018

Yes, too high on the resistors limits speed I've read? Assuming the MPU9250 core is the same for speed capability . . . swapping them - or if they are 4700 ohm - solder another 4700 over the ends to halve the resistance.

Mike - I only see one set of rtcOffset::

Code:

// Initialize rtcOffset
  rtcOffset = (float) micros() / 1000000.0;

That offset is probably where the negative space comes from on rollover?

If I got back to the PartSec() versus micros() - either from RTC or PPS based on cycle counter it would give better resolution and as noted way less compute intensive.

It naturally comes as a float as it is derived from a single division around the cycle counter - and it (re)sets each second based on RTC or PPS interrupt so it doesn't rollover. The whole part of seconds increments on each interrupt and the float portion is based of the cycle counter at that instant relative to cycle counter when it is asked for.

Code:

FASTRUN float PPS_PartSecf() { // 4J18
  return  (float)(((int32_t)ARM_DWT_CYCCNT - PPS_cCntPri) / (float)PPS_cAVG);
}

I started it with RTC before I got PPS - then moved to PPS - and made some effort to fall back to RTC when PPS was lost - but that was not going smoothly. The RTC isn't temp compensated - but should be no less accurate than the onboard crystal - and never affected by NoInterrupts() where a millis() tick could be lost - it should be better.

It might use a CB for the PPS_cAVG running average ( since it does what I did ), and the one second _isr() resets PPS_cCntPri each second as the second count is incremented - the second part not shown here.

mjs513 · Apr 2, 2018

"That offset is probably where the negative space comes from on rollover?" It probably is when micros rollover the original zero point is no longer valid. Probably should have used elapsedMicros but who would have thought.

RTC is probably a good way to start unless everyone wants to modify their GPS's or get one with the GPS signal broken out.

defragster · Apr 2, 2018

It should be trivial to do - minor change to keep whole seconds. I wondered about appropriate formatting some weeks back before I moved on . . .

From what I see these are ALL the micros() calls in the code:

Code:

T:\tCode\_GPSimuDK\1Apr18\uNavINS_MPU9250_I2C_MAGV4\uNavINS_MPU9250_I2C_MAGV4.ino:
  112  
  113    // Initialize rtcOffset
  114:   rtcOffset = (float) micros() / 1000000.0;
  115    // Initialize timing variables for printing
  116    currentTimeMillis = millis();
  ...
  140  
  141    //Define RTC time for this iterationrt
  142:   rtcTime = (float) micros() / 1000000.0 - rtcOffset;
  143  
  144    if ( gps.read(&uBloxData) ) debugGPS();
  ...
  160        //Serial.println("newData=1");
  161        newIMUData = 0;
  162:       newIMUDataTimer = (float) micros() / 1000000.0f - rtcOffset;
  163        CntIMU++;

Only one use of elapsedMicros:

Code:

T:\tCode\_GPSimuDK\1Apr18\uNavINS_MPU9250_I2C_MAGV4\uNavINS.h:
  125      bool mag_error_in_ned = true;
  126      // timing
  127:     elapsedMicros _t;

And this use of that _t here:

Code:

T:\tCode\_GPSimuDK\1Apr18\uNavINS_MPU9250_I2C_MAGV4\uNavINS.cpp:
  183  
  184      /* initialize the time */
  185:     _t = 0;
  186      
  187      // initialized flag
  ...
  189    } else {
  190      // get the change in time
  191:     dt = (float)_t /1000000.0;
  192:     _t = 0;

If I'm not missing anything - the micros() could be replaced by PartSec() above where it returns current "second+fractional part" based on the last RTC clock tick.

And it seems the use of _t should be based off the IMU_isr() at time of interrupt arrival.

something like:

Code:

static volatile float imu_now=0;
imu_isr(){
  imu_now = PartSec();
  // ...
}

uNavINS::update(
static float LastImu = 0;
if (!initialized) {
  LastImu = imu_now;
  // ...
}
else {
  dt = imu_now - LastImu;
  LastImu = imu_now;
  // ...
}

mjs513 · Apr 2, 2018

I added the elapsedMicros _t and dt to resolve the issue of rollover in piece of the code. dt is suppose to be the time between IMU updates if I understand the EKF correctly so as long as there is no issue of the clock rolling over (doubtful) that should work but I will let Don confirm that one.

The other times I think Don added as a time index (rtcTime) and I think newIMUdataTime he uses to see if there is a loss of data between GPS and the IMU. So I think if you use the IMUIsr and GPSIsr routine you had it would be better but again this part of the code Don has to confirm his intentions. I am guessing.

Now I am going to bring up the fun stuff for you Tim - Chris O. finished his updates to the UBLOX library and he is able to change the PPS rate, he got it up to 24Mhz, but I wouldn't go that high - think TelephoneBill did a little study that showed that. I also found that the M8N has a High Data Rate option to dump GPS solution at 30Hz. That may or may not work - haven't checked it out. Both of these things are on my list of things to do.

Chris O. · Apr 2, 2018

Now I am going to bring up the fun stuff for you Tim - Chris O. finished his updates to the UBLOX library and he is able to change the PPS rate

Well I got

this:

Not finish yet, will release it later (issues), unless you guys need it now let me know.

uNav INS

Senior Member+

Senior Member+

Senior Member+

Senior Member+

Senior Member+

Well-known member

Well-known member

Senior Member+

Senior Member+

Senior Member+

Well-known member

Well-known member

Senior Member+

Senior Member+

Well-known member

Well-known member

Senior Member+

Well-known member

Well-known member

Senior Member+

Senior Member+

Senior Member+

Senior Member+

Senior Member+

Well-known member