Skip to content
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

Navigator: Resume mission with last flight speed #21714

Merged
merged 6 commits into from
Aug 8, 2023

Conversation

sfuhrer
Copy link
Contributor

@sfuhrer sfuhrer commented Jun 12, 2023

Based on #21710

Solved Problem

On resuming a mission, the flight speed was reset to the vehicle default. Especially for surveys this is not acceptable.

Solution

Further extend the ideas from #21710: some mission items contain not momentary setpoints (like go to point A), but instead contain settings that are then relevant for the whole mission (or until over-turned by a negating command).
DO_CHANGE_SPEED items belong to the same category as eg camera settings: you want to re-apply them when resuming a mission.

I've further removed the separated mc/fw stored flight speeds, as it's only ever needed to store the speed of the current mode we're in, and we reset it during VTOL transitions.

Changelog Entry

Feature: Resume mission with flight speed from previous mission items
Documentation: Need to clarify page ...

Alternatives

@sfuhrer sfuhrer requested a review from RomanBapst June 12, 2023 16:34
@sfuhrer sfuhrer changed the title Pr store mission speed main Resume mission with last flight speed Jun 12, 2023
@sfuhrer sfuhrer changed the title Resume mission with last flight speed Navigator: Resume mission with last flight speed Jun 12, 2023
@DronecodeBot
Copy link

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/set-mission-speed-being-ignored-upon-starting-auto-flight/32705/2

@sfuhrer sfuhrer force-pushed the pr-survey-mission-resume-main branch 4 times, most recently from 25370a4 to 63e1d42 Compare June 20, 2023 09:14
Base automatically changed from pr-survey-mission-resume-main to main June 20, 2023 12:38
@sfuhrer sfuhrer force-pushed the pr-store-mission-speed-main branch from 039e4cd to 3fb1998 Compare June 23, 2023 10:06
tstastny
tstastny previously approved these changes Jun 23, 2023
Copy link

@tstastny tstastny left a comment

Choose a reason for hiding this comment

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

I left some questions, just because navigator is quite nebulous to me. But otherwise great that we are reducing complexity where possible.

src/modules/navigator/mission.cpp Outdated Show resolved Hide resolved
src/modules/navigator/navigator.h Outdated Show resolved Hide resolved
src/modules/navigator/mission.cpp Show resolved Hide resolved
@tstastny
Copy link

will likely need this in before the beaglebone is passing #21749

Copy link
Contributor Author

@sfuhrer sfuhrer left a comment

Choose a reason for hiding this comment

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

Addressed two of your comments with new commits @tstastny

src/modules/navigator/navigator.h Outdated Show resolved Hide resolved
src/modules/navigator/mission.cpp Outdated Show resolved Hide resolved
src/modules/navigator/mission.cpp Show resolved Hide resolved
…mode

This stored cruising speed setpoint is reset on mode change and
after a VTOL transition.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Speed changes in a mission are handled directly in the position controllers,
and no longer in Navigator.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
@sfuhrer sfuhrer force-pushed the pr-store-mission-speed-main branch from 4948bb7 to 0b18bd5 Compare July 17, 2023 13:55
@sfuhrer
Copy link
Contributor Author

sfuhrer commented Jul 17, 2023

Removed some speed handling logic from Navigator, and then rebased and force pushed. @tstastny would be good if you could make yourself a picture here and comment if you agree with the general direction of it (removing speed logic from Navigator).

@sfuhrer sfuhrer merged commit 56dd1dc into main Aug 8, 2023
83 of 85 checks passed
@sfuhrer sfuhrer deleted the pr-store-mission-speed-main branch August 8, 2023 11:22
@hamishwillee
Copy link
Contributor

I think it is an excellent feature to start from the previous setpoint. Can you update the docs @sfuhrer ?

I was thinking perhaps a very short section on pausing and resuming the mission in https://docs.px4.io/main/en/flight_modes/mission.html#mission-mode. Make it clear this applies to all frames.

antbre pushed a commit to BioMorphic-Intelligence-Lab/PX4-Autopilot that referenced this pull request Sep 14, 2023
* Navigator: DO_CHANGE_SPEED: only store sinlge cruising_speed_current_mode

This stored cruising speed setpoint is reset on mode change and
after a VTOL transition.

* Navigator Mission: replay DO_CHANGE_SPEED items when resuming mission

* Navigator: remove cruising_speed_sp_update()

Speed changes in a mission are handled directly in the position controllers,
and no longer in Navigator.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants