search

LSTS / dune

DUNE: Unified Navigation Environment
Other
111 stars 119 forks source link

Possible bug in PathController, now that EstimatedState.z == 0 #101

Closed krisgry closed 6 years ago

krisgry commented 7 years ago

Hi, We have had some problems with the longitudinal path controllers, and the start of a plan, so I did some digging through the PathController code, and found a strange line that I would like to discuss.

We are using LOS guidance in the longitudinal plane, which uses TrackingState start.z to create a line to the first WP. However, start.z is now zero until the first WP is reached. I believe that this issue has risen after the change to updating height and not z in EstimatedState. A change that seems to solve the problem for me is by changing https://github.com/LSTS/dune/blob/448c928f6ada06c38feef5f2e7210461a530da5f/src/DUNE/Control/PathController.cpp#L322 to m_pcs.start_z = m_estate.height - m_estate.z

I can reproduce this very well locally, but that is through the use of our longitudinal path controllers. I could not find any longitudinal PathControllers (Path/Height is a periodic task, and therefore does not have this problem). If you have any suggestions as to how I can reproduce this with any LSTS controllers, I'll happily try :) (Seems like ts.start.z is not used anywhere but in our code?)

josebraga commented 7 years ago

Hi Kristoffer,

Looking at code/logs, you are correct, it seems we've been filling start_z when plans start with incomplete information. I guess this is legacy to the time, many years ago when z used to be either depth (AUVs) or altitude from ground (UAVs), if I am not mistaken.

Are you filling m_pcs.start_z_units with IMC::Z_HEIGHT ?

Just one thing, EstimatedState.{x/y/z} is a NED displacement, so wouldn't the more accurate way to fill start_z be:

m_pcs.start_z = m_estate.height + m_estate.z ?

Thanks for the issue :)

krisgry commented 7 years ago

Hi Jose, Thanks for the quick feedback.

I have not filled start_z_units explicitly, since this was more of a quick fix. I guess that setting it to dpath->start_z_units make sense?

In terms of "height ± z", I'm fairly sure that it should be height - z. Height is positive upwards, while z is positive downwards.

What is the way forward from here? Should I file a merge request?

josebraga commented 7 years ago

Hi,

I believe for this case it makes sense to copy start_z from DesiredPath. Then, in your controller you would fill it as you see more fitting. The following start_z points (from following maneuvers) would still copy end_z to start_z. Does this fit your needs?

Since this has no practical effect on the controllers we are using I guess you can propose the solution fits you best and file a pull request.

Indeed, height-z makes more sense as they are opposite directions :D