When starting a new mission (or restarting the current mission) in AUTO mode I’ve noticed that there is a short reduction in speed (pictured below). In the example below I first command a mission with a single waypoint at 5m/s before reuploading the same mission and restarting with MAV_CMD_DO_SET_MISSION_CURRENT. On restart the speed drops to around ~2.5ms before returning to the commanded speed. In this scenario I would like the vehicle to maintain its current velocity (i.e. hold a steady 5 m/s).
This is a bit of a contrived example, but the intent is to keep the vehicle in AUTO mode and repeatedly command new waypoint routes from an external source.
The mission is composed of MAV_CMD_NAV_WAYPOINT and MAV_CMD_DO_CHANGE_SPEED mission items. These commands are all sent via mavlink. I’m running rover 4.5.1 in SITL. Other than this issue speed seems to track as expected with MAV_CMD_DO_CHANGE_SPEED items in a single mission.
I suspect this has something to do with re-initialization of wp nav controller on entering AUTO mode, but I figured I would ask around before going down too much of a rabbit hole.
Any suggestions or possible workarounds? I’m also happy to dig into the issue a bit more and submit a PR, but any advice on where to look first - or any stupid mistakes I’m making - would be greatly appreciated.
I think I figured out what is going on here. In AR_WPNav.cpp there is a call to set _scurve_this_leg (_scurve_this_leg.calculate_track...) within set_desired_location that is called when I restart the mission. This function seems to assume an initial velocity of 0.
Within copter there is a block that accounts for non-zero initial velocity with _scurve_this_leg.set_origin_speed_max(origin_speed);. I implemented a change locally basically mirroring that logic and it seemed to resolve my issue.
Is there a reason for this difference between copter/rover? Otherwise happy to send a PR on this if the update seems reasonable.
@Yuri_Rage@rmackay9 calling your attention to this … code diff betw copter and rover? I have seen that when navigating S-curves, rover goes slowly if there are waypoints close to each other. Right now my understanding is that this is because rover limits its speed so that it can arrive at the next-next waypoint at speed 0 (i.e. that it can decelerate to 0 by the time it reaches the next-next waypoint). But perhaps this observation by boats-hai sheds additional light on rover’s strangely slow navigation through waypoints which are close together? Of course, maybe i’m way off…
Copter and Rover are very similar so I suspect the change you’re suggesting will work fine and yes, please feel free to submit a PR and just include evidence of testing.
BTW, another options instead of using Auto mode is to use Guided mode. If you’re only providing a single target location at a time and/or the location is changed very often (e.g. at 1hz) then Guided will probably work better. We have some details here on the wiki.
Re the rover going slowly, I think you’re right that it is because S-curves (in both copter and rover) only look two waypoints ahead and it must be able to stop before reaching the 2nd wp.
