I have a few cases where I need to be able to pause (and sometimes resume) a waypoint mission. What I mean by pausing is “continue flying on the path of the current waypoint mission, but reduce speed to 0”.
Note that this is different from BRAKE mode in that BRAKE continues flying in a straight line.
So this would be setting the waypoint speed to zero which we don’t currently support I think. We also don’t currently support flying back to the exact point on the mission where it was interrupted … instead when the vehicle is switched back to Auto it will still head to the next waypoint but it will start from it’s current position instead of getting back on the line. There is a to-do item on the issues list to add this enhancement though.
I don’t need to return to the point where the mission was interrupted. I just need to reduce speed until the vehicle is stationary and then hold position. When resuming flight, just continue to the next waypoint.
If we were to implement it ourselves, can you recommend the right way to do it? Another mode? A sub-mode of auto? Something else?
You don’t need to create a new flight mode. You can write a script using pymavlink, dronekit or in Mission Planner python interpreter to send MAV_CMD_DO_PAUSE_CONTINUE. Please look at the wiki.
MAV_CMD_DO_PAUSE_CONTINUE doesn’t seem to be implemented in ArduCopter. Am I wrong?
I have never needed so didn’t try it before. You can try it in SITL.
Can’t you just switch to loiter or position hold and then back to auto?
Make sure MIS_RESTART=0
I don’t think either loiter or poshold attempt to keep to the waypoint mission’s path.
They do not. You seemed to imply that wasn’t critical. Both modes do hold a stationary position, though, so if the copter was on course when the mode change was initiated, it should remain reasonably so for the duration of the loiter/hold.
I’m not seeing MAV_CMD_DO_PAUSE_CONTINUE in the copter implementation, either. You could, perhaps, switch to GUIDED mode with a target of present position. I’m not sure that would behave any better than loiter or poshold, though.
It appears you could also send a MAV_CMD_NAV_LOITER_UNLIM command, but you’d have to briefly switch flight modes to exit the loiter.
Lastly, perhaps you could script a method to use MAV_CMD_NAV_LOITER_TIME and continue incrementing/updating the time value to sustain the loiter as needed.
(Yet another) EDIT:
This really piqued my curiosity. I’m about 75% done with a Lua script that will do the following upon execution:
- Command the slowest acceptable WPNAV_SPEED (20 cm/s) and await deceleration
- Once decelerated, compute the closest on-course point
- Command a LOITER_UNLIM at that point until given the command to resume
- Resume the mission at previous speed
@msasha, I’m not sure if your autopilot supports Lua scripting, but this proof of concept script is working fairly well in SITL for me (Copter 4.1.1).
To use it, you’ll need SCR_ENABLE=1 and a free RC channel on a 2 or 3 position switch, assigned to RCx_OPTION=300.
Because I cannot demand zero velocity without switching out of AUTO mode, there may be a very slight backtrack after the deceleration phase, mostly manifested as a mild yaw toward the previous waypoint.
If the Copter is off course when the pause command is issued, it will slowly correct back to course on a nearly perpendicular trajectory during the LOITER_UNLIM phase. I assume that’s desirable (i.e., the whole reason for doing this is to keep the Copter as close to the intended flight path as possible).
Also, depending on vehicle tune, it may be necessary to increase the
SPEED_THRESHOLD value a bit in order to consistently trigger the state change from deceleration to pause.
A word of caution - if you use the script as is, your waypoint mission could be permanently altered if you switch out of AUTO mode before resuming the mission. Also, there is little to no error checking, and my testing has only been thorough enough to prove the concept. I can polish it up a little if you think there’s merit to continuing down this path. I recommend testing only in SITL for now, and we can explore a more robust version soon if you like.
SmartPause.lua (5.3 KB)
I created a pull request for copter to support MAV_CMD_DO_PAUSE_CONTINUE in AUTO mode. You can find it in here. Try it. It works in SITL.
Alexander - When I read your post, I was thinking of the ability to simply enter “loiter” flight mode in the middle of a mission - which would stop your vehicle in place along it’s route to the next waypoint - and then when you’re ready, enter “auto” flight mode again to resume.
Of course maybe you want this done automatically via software, not by pilot commands.
Perhaps as you say “reduce speed to 0” there is something specific about the way the vehicle slows to a stop that is part of your criteria.
I’ve not personally had the chance to interrupt an “auto” mission with “loiter” and then resume with “auto” again - but from what I’ve read, it works.
Perhaps I haven’t explained myself clearly enough.
When approaching a waypoint in a mission, the vehicle will start turning towards the next waypoint, thus “cutting” the corner. If you switch to brake or loiter in the middle of the turn, the vehicle will slow down, but will fly in a straight line from the moment of the switch.
What I need is for the vehicle to slow down while continuing to fly on the curve it was flying before.
The script I wrote can be modified to keep the loiter target closer to present position rather than calculating one that may cause deviation from the turn. The deceleration phase of that script will do exactly what you desire, but it may not quite behave as you want during the LOITER_UNLIM if you try to pause mid-turn.
Please let me know if that’s something you’d like to try. Otherwise, I’m not likely to put much more effort into the script.
It looks like the pull request above uses BRAKE mode, which won’t quite do what you want. Allowing a WPNAV_SPEED of zero would do it, as Randy says.
Setting line 19 of AC_WPNav.cpp to the following would probably work unless there are strange/unintended consequences that result from a zero speed value.
#define WPNAV_WP_SPEED_MIN 0.0f // minimum horizontal speed between waypoints in cm/s
I don’t have a build environment set up at present, but I need to install one. If I have a chance this weekend, I’ll test that line and create a pull request if it’s successful.
I tried changing AC_WPNav.cpp and tested with SITL. SCurve navigation does NOT like zero speed - it causes a core dump. Simply allowing WPNAV_SPEED=0 is an invalid tactic without further refinement.