GPS sensor

Development & General Chat for the superb openxvario project.

Moderator: rainer

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sat Apr 09, 2016 9:03 am

I've been looking but I can't really find any info on getting yaw rate from the sensor, I'm sure it's possible though, but still I think working it out from the yaw we have already may work well enough.
I flew yesterday morning in calm conditions with the yaw rate calculated in tx and it is definitely working as a indication of roll, continuous turns were indicated better, just slow and jumpy due to the nature of the s.port data.


BTW I have got the compass gy271 installed and working in the taranis now, will test fly later today but statically it is working allowing me to use the compass heading to orientate the radar according my own heading.
It is quite sensitive to tilt angles though ..

mstrens
Posts: 1056
Joined: Fri Dec 27, 2013 7:49 pm
Country: -

Re: GPS sensor

Post by mstrens » Sat Apr 09, 2016 11:34 am

I put on githuub a new version where TEST_1 is filled with yaw rate.
I calculate it once every 200 msec (based on a formula like (current yaw - previous yaw) / enlapsed time.
Value is in 1/100 of degre/sec.

I did not test it. At least it compiles ok.

Can you check it and let me know if it is ok.

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sat Apr 09, 2016 11:53 am

Thanks, will test and report back.

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sat Apr 09, 2016 2:41 pm

tested and something is wrong, I get altitude and v.speed going crazy, pitch roll and yaw all ok.
Yaw rate in test1 as T1 is zero when still, turning right it goes to 5000 or so , turning left it goes to huge numbers something like 2000000.

mstrens
Posts: 1056
Joined: Fri Dec 27, 2013 7:49 pm
Country: -

Re: GPS sensor

Post by mstrens » Sat Apr 09, 2016 5:36 pm

I just made a correction but I am not sure at all that it would become correct.
I do not understand why Alt and Vspeed are going crazy.
I still put this update on github.
I will try to test myself to morrow


nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sat Apr 09, 2016 6:55 pm

tried that version and still same.
I'll wait and see how you get on tomorrow.
Thanks.

mstrens
Posts: 1056
Joined: Fri Dec 27, 2013 7:49 pm
Country: -

Re: GPS sensor

Post by mstrens » Sun Apr 10, 2016 7:57 am

New version is on github.
I changed the way I calculate.
It seems that the program performs now what is expected.
Still I see several issues:
I calculate every 200 msec (5 X per second) the yaw rate.
It is done with a formula like (current yaw - previous yaw) * 5

1) Yaw has no decimal; so yaw rate you will be in multiple of 5 (0, 5, 10, 15). Therefore I sent it now in degree / sec instead of 1/100 of degree/sec)

2) When yaw cross 180°, yaw changes form positive to negative. This can give wrong values. e.g. imagine that the plane rotates at 10 degree/sec.
So successive values of yaw will be e.g.
175 ,177 , 179 , -179, -177, - 175...
So the calculated yaw rate will be:
(177 - 175) * 5 = 10 (OK)
(179 - 177) * 5 = 10 (OK)
(-179 - 179) * 5 = 0 (NOK)
(-177 - -179) * 5 = 10 (OK)
(-175 - -177) * 5 = 10 (OK)

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sun Apr 10, 2016 11:17 am

Thanks,
Just tested on bench and it works, before I take apart plane to install I wondered if the update rate could be faster? Not a huge problem but would be nice if it was 10hz, I am not sure which I will use, but I have altered the script to allow either just yaw rate as roll or both roll and yaw rate added together.
probably each different plane I will use a different option..
minimoa can probably be just yaw rate if update was faster.

mstrens
Posts: 1056
Joined: Fri Dec 27, 2013 7:49 pm
Country: -

Re: GPS sensor

Post by mstrens » Sun Apr 10, 2016 12:03 pm

in line 892 of .ino file, there is :
if (currentMillis >= previousYawRateMillis + 200 ) {

In this line 200 means 200 msec. It is the delay between 2 calculations.

If you want a new yaw rate every 100 msec, just replace 200 by 100.

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sun Apr 10, 2016 12:06 pm

Great thanks

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sun Apr 10, 2016 1:06 pm

OK, I tried that, it works but is very jumpy, not precise enough, if I change to this

currentMillis = millis() ;
if (currentMillis >= previousYawRateMillis + 1000 ) {
test1.value = (yaw.value - previousYaw) ; // calculate yaw rate in degree / sec

precision is good and not jumpy but obviously slow update rate.
if I used that in combination with the roll I could probably see it working sort of due to that fact that the roll is responsive and responds more to control inputs..

but to get smooth and precise results I think decimals are going to be needed and maybe even some filtering.

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sun Apr 10, 2016 1:18 pm

I tried this and it seems acceptable.
currentMillis = millis() ;
if (currentMillis >= previousYawRateMillis + 200 ) {
test1.value = (yaw.value - previousYaw) *5 ; // calculate yaw rate in degree / sec

mstrens
Posts: 1056
Joined: Fri Dec 27, 2013 7:49 pm
Country: -

Re: GPS sensor

Post by mstrens » Sun Apr 10, 2016 5:27 pm

If you want, you can add some smoothing. It is very easy.
Replace line with
test1.value = (yaw.value - previousYaw) *5 ;

by
test1.value += 0.1 * ( ( (yaw.value - previousYaw) *5 ) - test1.value ) ;

You can adapt the smoothing changing 0.1 by another value.
If you increase 0.1, then soothing increase (less noisy).
If you replace 0.1 by 1, then there is no smoothing anymore.
So, never exceed 1.

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sun Apr 10, 2016 6:09 pm

Thanks , I will try it out and experiment with some different values.

Going back to the original 6050 arduino program ( the one with tea pot demo) I did a test and get decimal places from that , I presume you have done away with the decimal places for memory purposes?

mstrens
Posts: 1056
Joined: Fri Dec 27, 2013 7:49 pm
Country: -

Re: GPS sensor

Post by mstrens » Sun Apr 10, 2016 6:35 pm

I put yaw, pitch, roll in integer fields because telemetry protocols request that values are integers
Still, it is easy to get decimals. I just have to add a multiply by 100 in the formula to get 2 decimals.

In oXs_imu.cpp, in line 299 there is curently :

yaw.value = atan2(2 * q.x* q.y - 2 * q.w * q.z, 2* q.w * q.w + 2 * q.x * q.x - 1) * radians_to_degrees ; // yaw
pitch.value = atan(gravity.x / sqrt(gravity.y*gravity.y + gravity.z*gravity.z)) * radians_to_degrees; // Pitch
roll.value = atan(gravity.y / sqrt(gravity.x*gravity.x + gravity.z*gravity.z)) * radians_to_degrees; // Roll

You could change it in (in order to get the result in 1/100 of degree)
yaw.value = 100 *atan2(2 * q.x* q.y - 2 * q.w * q.z, 2* q.w * q.w + 2 * q.x * q.x - 1) * radians_to_degrees ; // yaw
pitch.value = 100 * atan(gravity.x / sqrt(gravity.y*gravity.y + gravity.z*gravity.z)) * radians_to_degrees; // Pitch
roll.value = 100 *atan(gravity.y / sqrt(gravity.x*gravity.x + gravity.z*gravity.z)) * radians_to_degrees; // Roll
Then Yaw rate would also be in 1/100 of degree/sec

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sun Apr 10, 2016 6:50 pm

the previous formula gives a nice smoothing effect obviously at the expense of reaction time.
Unfortunately though it shows up the problem of crossing the south heading, each time it crosses south the horizon spins lol...
I think I will try out the original version with no smoothing updating at 200ms, seems a good compromise and especially when using d series rx the update rate wont be an issue.
Just need some flying weather again, was nice early this morning but very cold, I had ice form on the wing of my raptor!

edit I see your post,
I will try that then and see if I can get the yaw rate better from that.

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sun Apr 10, 2016 7:10 pm

yes! much better now ..

with the *100 in the line as you said and

this
if (currentMillis >= previousYawRateMillis + 100 ) {
test1.value = (yaw.value - previousYaw) *0.05 ; // calculate yaw rate in degree / sec
the results are perfect once I got rid of the *100 in my lua script, and the yaw itself is smoother too.

tested on the x series d16 rx mode.

mstrens
Posts: 1056
Joined: Fri Dec 27, 2013 7:49 pm
Country: -

Re: GPS sensor

Post by mstrens » Mon Apr 11, 2016 7:39 am

I presume you apply the correct formula using "test1.value += (yaw.value - previousYaw) *0.05 ;"
and not
"test1.value = (yaw.value - previousYaw) *0.05 ;"
So with += instead of =

If you want to smooth the yaw too, you can apply the same principle replacing
yaw.value = 100 * atan2(2 * q.x* q.y - 2 * q.w * q.z, 2* q.w * q.w + 2 * q.x * q.x - 1) * radians_to_degrees ; // yaw
by something like
yaw.value += ( (100 * atan2(2 * q.x* q.y - 2 * q.w * q.z, 2* q.w * q.w + 2 * q.x * q.x - 1) * radians_to_degrees ) - yaw.value) * 0.05 ;

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Mon Apr 11, 2016 8:27 am

No I just used the + as in my post, the reason it works is the numbers are higher and have more data and so changing more often.
This was the way you had it before more or less, I just altered the amount to multiply by to suit the fact that yaw is now 100 times greater in value.
I don't think any smoothing is needed now with the more precise yaw data ..
I will try adding the *100 to pitch and roll today too to see how that goes, I have to multiply by 100 in my script anyway to get it to make sense .


On a different note , I was thinking about the fact that we are using gravity to calibrate the sensor and the centrifugal forces effecting gravity are causing this problem , I also noted yesterday whilst in a tight thermal turn a sudden big drop in the v.speed which did not match the craft actual dropping in height, might this be related to the gravity calibration?
Thinking out loud I wonder if using a different method to calibrate might be possible, maybe using yaw rate and v.speed and maybe even roll and pitch rate, if all these were stable and especially v.speed within a given amount and we are not inverted then calibrate at that point.
Just an idea, what do you think? Could it work?

mstrens
Posts: 1056
Joined: Fri Dec 27, 2013 7:49 pm
Country: -

Re: GPS sensor

Post by mstrens » Mon Apr 11, 2016 11:14 am

I think that using this code is wrong:
if (currentMillis >= previousYawRateMillis + 100 ) {
test1.value = (yaw.value - previousYaw) *0.05 ;

In the "if", you have a parameter 100: it means that the rest of the code is performed about every 100 msec so about 10 X per sec.
So, in this case, the next line should be (if you want a value in degree / sec)
test1.value = (yaw.value - previousYaw) * 10 / 100 ;

Alternative : change 100 by 200 in order to calculate every 200 msec (and then keep * 0.05 on second line).

Note: you can get a higher precision on the yaw rate if you keep the value in 1/100 of degree/sec instead of converting in degree / sec.
The value transmitted to Tx will be 100 X bigger because it is an integer but you can divide it on Tx side in order to get it in degree/sec with 2 decimals.

It is not clear for me what you mean by "calibrate".
Normally "calibrate" means a (one time or periodic) process in order find the offsets inside the imu (on acc and gyro).
Acc offsets are normally defined once as explained in the description.
Gyro offset are automatically performed by the program (DMP algorithm) that runs inside the 6050 chip.
The dmp algorithm combines informations from Acc and from Gyro in order to provide the 3D orientation (and rotation) of the 6050 device.
This algorithm assumes that over a long time (I do not know how long it is) the average of all acceleration is equal to gravity. This assumption is based on the idea that a mobile can't accelerate continously (otherwise velocity would become very high).
Still when turning continously, the centrifugal force adds itself to gravity for a long time and so can generate false 3D orientation coming out of the DMP algoritm.
I think that some multicopter project do not use the DMP algorithm and use their own algorithm to calculate 3D position of the 6050. They can then avoid realigning 3D position with gravity vector when yaw rate and/or acceleration is to big.
In order to save memory/CPU in oXs I opted for using DMP algorithm inside 6050.
Still it could be that it generates some false measurements.
Experiment is the best way to see if the error is acceptable or not.
One option could be to calculate 2 vertical speeds (one only with the baro and one with baro + imu) and to transmit the one from baro alone when yaw rate (and/or accelerations) is to high.

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Mon Apr 11, 2016 11:49 am

OK thanks,
This is the first time I had the v.speed error and was circling very tight , I don't think it will be a problem in general.
As for the yaw pitch and roll etc, it would be nice if they could be correct but not really important , I can probably get a good enough indication of pitch and roll from v.speed and yaw rate to fly home if needed if the current method proves unreliable.
And yes its all about experimenting, very interesting stuff.

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Tue Apr 12, 2016 9:34 am

Flew this morning and the yaw rate works great as an indication of roll, I tried just yaw rate on it own and it was a little laggy as an indication of roll and I tried both roll and yaw rate together added and this worked well with just a little overshoot but both methods are very useable all deciding which to use with depend on the aircraft type.

Unfortunately the sensor stopped working again like it did on a previous version, I landed and cycled power and it worked immediately again.
Only happened once in the 50 minutes I was flying.

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Tue Apr 12, 2016 1:15 pm

I've had to change the *100 part because of hub telemtry only alowing up to 30 in acc values compared with s.port which was happy with more..
I now multiply the acc values by 10 instead of 100 and then multiply them in the script by 10 and got rid of the *.1 in the yaw rate calculation.
It still works ok though so I am happy with this.

I guess I could have done it by removing the *10 in the sending of hub data and dividing the s.port data by 10 instead but this is working so..

Thanks Mstrens!

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Thu Apr 14, 2016 12:38 pm

I flew a few times now with different methods of viewing the heading/yaw on taranis,
In the end i found the best method was to just calibrate the yaw when new GPS data was available, and use yaw as heading in my script.
The reason was in windy conditions the yaw alone could often be wrong by 90 degrees if only calibrated from GPS when flying level and straight.

I also note that the yaw rate is quite noisy using it as roll angle the way I use it now, putting smoothing on it made it appear to flip around on its axis every time it passed a certain heading.
I'm wondering if smoothing the yaw pitch and roll would help ?

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Thu Apr 14, 2016 6:34 pm

I found a solution to smoothing the yaw rate, this just ignores the wrong values and sends previous values.

#if defined (VARIO) && defined (USE_6050)
static int32_t previousYaw ;
static uint32_t previousYawRateMillis ;
uint32_t currentMillis ;
currentMillis = millis() ;
if (currentMillis >= previousYawRateMillis + 100 ) {
if (yaw.value-previousYaw<340&&yaw.value-previousYaw> -340){
test1.value += 0.5 * ( ( (yaw.value - previousYaw) *1.0 ) - test1.value ) ;//this will give smoothing, first number between .1 and 1 lower number gives more smoothing
}

test1.available = true ;
#define DEBUGYAWRATE
#ifdef DEBUGYAWRATE
//Serial.print(lastImuInterruptMillis) ; Serial.print(" ") ; Serial.print( previousYawRateMillis ) ;Serial.print(" ") ; Serial.print( yaw.value ) ; Serial.print(" ") ; Serial.print( test1.value ) ; Serial.print(" ") ; Serial.println(oXs_MS5611.varioData.absoluteAlt.value) ;
Serial.print( currentMillis ) ;Serial.print(" ") ; Serial.print( yaw.value ) ; Serial.print(" ") ; Serial.print( test1.value ) ; Serial.print(" ") ; Serial.println(oXs_MS5611.varioData.absoluteAlt.value) ;
#endif

previousYaw = yaw.value ;
previousYawRateMillis = currentMillis ;

}

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Sun Apr 17, 2016 6:38 pm

I see on github you put version with bug fixes for yaw rate, in particular changing sample rate to 200ms for yaw, etc, I just wondered why and if my results will be better or worse with the yaw rate calculation now?
Thanks.

mstrens
Posts: 1056
Joined: Fri Dec 27, 2013 7:49 pm
Country: -

Re: GPS sensor

Post by mstrens » Mon Apr 18, 2016 6:54 am

The version on github is 8 days old.
The changes were the results of your first tests (saying e.g. that altitude was going crazy).
It was important to put this fix on github because it was really a bug that would have an impact on all users.
At this time, you asked for 5 updates per second and this explain the 200 ms refresh rate.
Later I explained here which minor changes you could try (refresh rate, adding decimals, smoothing).
I was waiting the end of your experimentations in order to put on github a version that best fit your expectattions and could be used by all users.

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Mon Apr 18, 2016 7:44 am

Oh I see now, yes thanks for that!
You mentioned that smoothing or filtering of the yaw pitch and roll could be done if needed, would the same smoothing formula be needed on those as the yaw rate you suggested?
I think if those were filtered then the yaw rate might not need any filtering and react fast and smooth.

Another experiment I did on tx lua script was to get roll angle by roll = accy*accx,
The idea was that as the plane turned the nose up indication would be passed on to the displayed roll angle, this worked to some reasonable degree but when I climbed with motor it went wild and was also not any more responsive then the yaw rate and often not as accurate.

mstrens
Posts: 1056
Joined: Fri Dec 27, 2013 7:49 pm
Country: -

Re: GPS sensor

Post by mstrens » Mon Apr 18, 2016 5:52 pm

If you apply smoothing on yaw, then yaw rate will also be automatically smoothed (because yaw rate would normally be calculated on the smoothed yaw).
Yaw rate based on smoothed yaw will anyway react slowlier than if it would be calculated based on yaw (just because smoothed yaw reacts slowlier).

The easier way to smooth any value is to apply following formula
smoothed_value = smoothed_value + X * ( new_unsmoothed_value - smoothed_value)
where X is a value between 0 and 1
When X = 0, then the new_unsmoothed_value is not taken into account at all.
When X = 1, then the new_unsmoothed_value totally (100%) taken into account so it means that there is no smoothing at all.
A common value for X is between 0.05 and 0.3. The lower the value, the more smoothing you get.

If this is unclear, let me know.

nigelsheffield
Posts: 308
Joined: Fri Nov 08, 2013 9:56 pm
Country: -

Re: GPS sensor

Post by nigelsheffield » Mon Apr 18, 2016 8:40 pm

Thanks , yes it's clear, i will have play with various smoothing values on all yaw pitch and roll to see what is good compromise between smooth and speed.

Post Reply

Return to “OpenXVario - an open source vario supported by the open source firmwares!!”

Who is online

Users browsing this forum: No registered users and 6 guests