Skip to content

Return utm.alt as hmsl from gps helper function #1730

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 4 commits into from
Jun 19, 2016

Conversation

kirkscheper
Copy link
Member

Although there is no defined reference for the utm altitude, in paparazzi (most in fixedwing firmware) it is more often than not assumed that alt is hmsl. I updated the helper functions from gps->utm to return alt with hmsl.

Additionally, I added additionally checks for the gps valid mask. Notably, if both UTM and LLA positions are not set utm is returned as (0,0,0, zone). Also, if gps.hmsl is not available we use the ellipsoid alt minus the geoid height at the original origin of the flight plan.

@flixr
Copy link
Member

flixr commented Jun 16, 2016

it is more often than not assumed that alt is hmsl.

In all cases it should be documented if it is hmsl or ellipsoid or... if you find a case where this is not documented please fix it or create an issue for it.

@kirkscheper
Copy link
Member Author

The assumption is usually in the use not the setting, in quite a lot of the (vintage) code utm.alt is more often than not used synonymously with hmsl.

Additionally, the original helper didn't specify the alt at all (as far as I remember)...

@@ -355,10 +357,15 @@ struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
utm.north = gps_s->utm_pos.north / 100.0f;
utm.alt = gps_s->utm_pos.alt / 1000.f;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should also use gps_s->hmsl here... as gps utm_pos.alt is above ellipsoid

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

hmmm... Well I am also inclined here to change the gps to hmsl.

Additionally, I just saw that the conversions from lla to utm don't change the alt from ellipsoid to hsml although utm is defined hsml there.

The current implementation is not coherent. You have strong a opinion on this?

@flixr
Copy link
Member

flixr commented Jun 16, 2016

hmmm... Well I am also inclined here to change the gps to hmsl.

What do you mean?

Additionally, I just saw that the conversions from lla to utm don't change the alt from ellipsoid to hsml although utm is defined hsml there.

Just having a LLA datum you don't have the information to set UTM alt above MSL...

@kirkscheper
Copy link
Member Author

kirkscheper commented Jun 16, 2016

What do you mean?

change the utm.alt to hmsl.

Just having a LLA datum you don't have the information to set UTM alt above MSL...

not sure what you mean here, as I tried to describe in my original description, the msl will not be very accurate but I provide the best estimate using the initial geoid separation at the origin as you don't have the geoid model onboard (as far as I know). This is no the most accurate computation of hmsl but is definitely more accurate than using an invalid gps.hsml as is currently possible in the current use of this function.

@flixr
Copy link
Member

flixr commented Jun 16, 2016

change the utm.alt to hmsl.

check what the old ublox4 report there...

not sure what you mean here, as I tried to describe in my original description, the msl will not be very accurate but I provide the best estimate using the initial geoid separation at the origin as you don't have the geoid model onboard (as far as I know). This is no the most accurate computation of hmsl but is definitely more accurate than using an invalid gps.hsml as is currently possible in the current use of this function.

The LLA to UTM conversion are generic, not only used for GPS.
So it simply copies the alt in LLA to UTM... the alt in LLA could be either above ellipsoid or above geoid.
And also always using the geoid separation at the local origin (or from the course model in pprz_geodetic_wgs84) is also not nice, since you might have a more accurate geoid separation from somewhere else...

@kirkscheper
Copy link
Member Author

And also always using the geoid separation at the local origin (or from the course model in pprz_geodetic_wgs84) is also not nice, since you might have a more accurate geoid separation from somewhere else...

Fair enough. The problem with the reality of how this function is used is that in the original code the gps.hsml was used as height but it was never checked if that was valid... This implementation (although not complete) is a clear improvement in the situation the gps.hsml is missing and has no change in the case it is in fact available.

I can make an issue to generate a more accurate geoid separation using your current position but in my mind that is a separate issue. As far as I see pprz_geodetic_wgs84 doesn't make the geoid separation easily available could be a future development.

check what the old ublox4 report there...

Ok, I won't touch that for now, more important issues to handle.

The LLA to UTM conversion are generic, not only used for GPS.

state currently says:
/**

  • Position in UTM coordinates.
  • Units x,y: meters.
  • Units z: meters above MSL
    */
    struct UtmCoor_f utm_pos_f;
    but is not always computed that way.

@flixr
Copy link
Member

flixr commented Jun 16, 2016

Regarding what ublox4 reports as UTM alt: it simply says "Altitude" while in other messages it clearly states whether it is height above ellipsoid or MSL...

As far as I see pprz_geodetic_wgs84 doesn't make the geoid separation easily available could be a future development.

There is the wgs84_ellipsoid_to_geoid_i function... you just have to keep in mind that is a rather coarse model...

Units z: meters above MSL

That just says that in the state interface the UTM alt is above MSL, but that of course doesn't mean that it is the case for every UtmCoor_f struct used in other places...

@kirkscheper
Copy link
Member Author

Haha, yea, I looked at the wgs84, that's why I said 'easy way' I have no
idea how to read a value from the matrix, I would have to make some
assumptions about how that array is arranged.

Abkut the state definition, take a look at the statecalpositionUtm...
On 16 Jun 2016 5:58 p.m., "Felix Ruess" [email protected] wrote:

Regarding what ublox4 reports as UTM alt: it simply says "Altitude" while
in other messages it clearly states whether it is height above ellipsoid or
MSL...

As far as I see pprz_geodetic_wgs84 doesn't make the geoid separation
easily available could be a future development.

There is the wgs84_ellipsoid_to_geoid_i
http://docs.paparazziuav.org/latest/group__math__geodetic__wgs84.html#gab74a66cf6cc775c3cf07620dd6abed0b
function... you just have to keep in mind that is a rather coarse model...

Units z: meters above MSL

That just says that in the state interface the UTM alt is above MSL, but
that of course doesn't mean that it is the case for every UtmCoor_f struct
used in other places...


You are receiving this because you authored the thread.
Reply to this email directly, view it on GitHub
#1730 (comment),
or mute the thread
https://github.com/notifications/unsubscribe/ACtEQ0q8abtI6C1hrt9CyhjOTu2IdZ6Cks5qMXKygaJpZM4I3Zpw
.

@flixr
Copy link
Member

flixr commented Jun 16, 2016

Apart from using NAV_MSL0 as a fallback (which doesn't make any sense as it's a fixed value at the flight plan location) it looks good to me.
The fallback should use ellipsoid alt + geoid separation (either use wgs84_ellipsoid_to_geoid_i or get it from the origin definition like e.g. in https://github.com/paparazzi/paparazzi/blob/master/sw/airborne/modules/air_data/air_data.c#L108)

About the state interface: you are right, but IMHO that should be fixed there (we could also add another conversion function that always assumes LLA ellipsoid alt and returns UTM hmsl alt using the hmsl value from the state interface).

@kirkscheper
Copy link
Member Author

get it from the origin definition like e.g. in https://github.com/paparazzi/paparazzi/blob/master/sw/airborne/modules/air_data/air_data.c#L108)

This is a bit of a frustrating comment as getting the separation from the origin is basically exactly the same as what I do now cause if you don't have gps the hmsl of the origin is set to the value from the flight plan (ltp_def.hmsl = NAV_ALT0;).

The fallback should use ellipsoid alt + geoid separation (either use wgs84_ellipsoid_to_geoid_i

For this to be worth it you would have to fly at least 5 degs of longitude or latitude with the model currently implemented, if you can do that with a gps that doesn't output a hsml measurement good on you.

To appease you I will add the wgs84_ellipsoid_to_geoid_i look up cause its not really worth any more discussion.

@flixr
Copy link
Member

flixr commented Jun 16, 2016

get it from the origin definition like e.g. in https://github.com/paparazzi/paparazzi/blob/master/sw/airborne/modules/air_data/air_data.c#L108)

This is a bit of a frustrating comment as getting the separation from the origin is basically exactly the same as what I do now cause if you don't have gps the hmsl of the origin is set to the value from the flight plan (ltp_def.hmsl = NAV_ALT0;).

Not exactly:

  1. right now you are setting alt to the geoid_separation (NAV_MSL0) instead of ellipsoid alt + geoid separation
  2. you are only considering the case where someone actually flies at the flight plan location, but many people use a default flight plan and just let GeoInit set it to their current location...

That is why in most other cases we "calc" the geoid separation from the currently used origin.

@podhrmic
Copy link
Member

Out of curiosity - what is wrong with wgs84_ellipsoid_to_geoid_i function? I am using it to convert ALT readings from INS (Vectornav) to HMSL (which is what we want), and it seems to be "close enough"?

@flixr
Copy link
Member

flixr commented Jun 16, 2016

@podhrmic there is nothing "wrong" with it, it's just that it only has a 10deg resolution... so depending on where you are that is too coarse (e.g. in the Alps).

@kirkscheper
Copy link
Member Author

Not exactly:

  1. right now you are setting alt to the geoid_separation (NAV_MSL0) instead of ellipsoid alt + geoid separation

check the code at https://github.com/paparazzi/paparazzi/blob/master/sw/airborne/subsystems/ins/ins_float_invariant.c#L228

The lla origin is set with (llh.alt = NAV_ALT0 + NAV_MSL0;), hmsl is set with (ltp_def.hmsl = NAV_ALT0;) and then the separation is computed with (state.ned_origin_f.lla.alt - state.ned_origin_f.hmsl;) so I am clearly missing something...

  1. you are only considering the case where someone actually flies at the flight plan location, but many people use a default flight plan and just let GeoInit set it to their current location...

This would require a gps which has hmsl, if you have that then none of this is an issue. That in itself looks like the gps valid bits are not checked: https://github.com/paparazzi/paparazzi/blob/master/sw/airborne/subsystems/ins/ins_float_invariant.c#L283

@flixr
Copy link
Member

flixr commented Jun 16, 2016

sorry, I missed the -=...

Also we have support for multiple GPS... so you could have one that has hmsl and one that doesn't...

And yes, it seems that the valid bits are not checked everywhere where they should as that is a more recent addition...

@kirkscheper
Copy link
Member Author

I briefly considered the multigps issue when I started but decided to ignore that for now as I don't have an obvious solution for that atm and it also would have been an issue for the previous implementation which wasn't handled.

I am not attempting to solve this issue forever with the best possible method, I just saw an opportunity for a possible bug or user misuse and thought that this approach would be slightly better in some use cases and not worse in any use case I could think of.

@flixr
Copy link
Member

flixr commented Jun 16, 2016

It's great, don't doubt that!
I just wanted to consider as many cases as possible, since experience/history shows that it will otherwise be forgotten again... ;-)

Looks good to me now.

@flixr
Copy link
Member

flixr commented Jun 17, 2016

@gautierhattenberger do you know what exactly the ublox4 return as UTM alt? Any thing else to do here?

@gautierhattenberger
Copy link
Member

Documentation is not available anymore for ublox4, but we can assume that:

  • it is returning hmsl
  • they are not used anyway (but if someone really likes them, I have 3 of them full of dust in a drawer)

I guess the rest is fine

@kirkscheper
Copy link
Member Author

I noticed a couple other issues with the ground alt and setting the utm_origin but not really with the helper function. Shall I include that here or make a new pr?

@flixr
Copy link
Member

flixr commented Jun 17, 2016

If ublox4 also returns hmsl, then it would probably make sense to change the gps utm_pos struct to have hmsl alt after all as you proposed before...
@kirkscheper Do you still want to do that in this PR?

I noticed a couple other issues with the ground alt and setting the utm_origin but not really with the helper function. Shall I include that here or make a new pr?

Please make a new PR for that.

@kirkscheper
Copy link
Member Author

yea, no prob to both

@kirkscheper
Copy link
Member Author

@flixr what was the idea with your addition from dec last year at https://github.com/paparazzi/paparazzi/blob/master/sw/airborne/modules/gps/gps_ubx#L131 toll L135? Don't really get what you mean there in the comment and how the code will every be run...

@flixr
Copy link
Member

flixr commented Jun 17, 2016

Oh, it should be what the comment actually says:

/* with UTM only you do not receive ellipsoid altitude, so set only if no valid lla */
if (!bit_is_set(gps_ubx.state.valid_fields, GPS_VALID_POS_LLA_BIT)) {
  gps_ubx.state.lla_pos.alt = gps_ubx.state.utm_pos.alt;
}

thanks for noticing that!

@flixr
Copy link
Member

flixr commented Jun 17, 2016

Also that is probably converted from a previous version without the valid bits...
If the valid bits are properly checked by each consumer, that part can be totally removed...

@kirkscheper kirkscheper force-pushed the utm_hsml branch 3 times, most recently from 01ca625 to ca6bf37 Compare June 17, 2016 12:09
@flixr
Copy link
Member

flixr commented Jun 19, 2016

Note to self: good to merge after @kirkscheper also added the gps utm alt to be in hmsl...
No rush there though... thanks again for your great efforts to clean this up @kirkscheper !

@kirkscheper
Copy link
Member Author

Check the most recent commit, that should have all the conversions for the gps.utm_pos. I can check it again on Monday to make sure that I got all the instances.

@flixr
Copy link
Member

flixr commented Jun 19, 2016

Right, sorry - forgot to refresh the page...
Looks good to me.

@flixr flixr merged commit e0ed31a into paparazzi:master Jun 19, 2016
@kirkscheper kirkscheper deleted the utm_hsml branch June 20, 2016 07:47
wilcoschoneveld pushed a commit to wilcoschoneveld/paparazzi that referenced this pull request Jun 22, 2016
* Return utm.alt as hmsl from gps helper function

* also convert to hsml when valid gps

* added wgs84 geoid separtation lookup to gps->utm helper

* convert gps.utm to hmsl from ellipsoid
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants