-
Notifications
You must be signed in to change notification settings - Fork 13.7k
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
mc_pos_control: Fix bug causing flyaway when GPS gained after takeoff #7983
Conversation
The change in origin when GPS was gained after takeoff was being used to shift the set point despite the previous origin being invalid.
8470d70
to
39aab22
Compare
@@ -856,7 +858,7 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) | |||
void | |||
MulticopterPositionControl::update_ref() | |||
{ | |||
if (_local_pos.ref_timestamp != _ref_timestamp) { | |||
if (_local_pos.ref_timestamp != _ref_timestamp && _first_origin_set) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@priseborough This PR just works for the situation where we use flow at the beginning and then gain GPS. The reason is that this line will never be fully true. If _first_origin_set
is true, _ref_timestamp
has been set to _local_pos.ref_timestamp
. Hence we will never enter this code block.
For the normal simulation (make posix_sitl_default gazebo_iris
) the quad climbs into the sky when you run commander takeoff
because _ref_alt
has never been set.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
OK, I will review and fix tomorrow.
The long term fix would be to move to local coordinates only, right? I think, we need to push for that. |
Agree - that will solve this type of problem, but would require the waypoints to be converted to local coordinates. It would also avoid the need for the expensive local to global transformation needing to be done at every time step. It would only need to be done once for each waypoint. |
3f4d574
to
1521a4c
Compare
The last commit is the simplest way I can think of to prevent the reference point changing during flight, which is not safe with the way the controller works in global coordinates. This change is compatible with the ecl/EKF because it will only set the origin once power cycle. I've checked LPE and also appears to only set the origin once (@jgoppert can you confirm this?). If it is absolutely necessary to retain the ability to change reference point in flight, then we can modify the logic to only allow it when in a waypoint driven flight mode. |
@priseborough This seems to work for the EKF2! 👍 @jgoppert Controller-wise it also seems to work for the LPE -> no jumps in setpoints. However, I see a jump in the estimated position when switching to use GPS as well (see below). Any ideas why that happens? |
The change in origin when GPS was gained after takeoff was being used to shift the set point despite the previous origin being invalid.
@priseborough FYI