I have built an autonomous pile-driving machine controlled by Ardupilot. I am using GPS in Moving Baseline mode with RTK corrections from a reference station.
Most often during operation, the machine should move in a straight line to the next hammering point. In my case, I assume that I send a 1-point mission via Mavlink and expect the vehicle to accurately reach and stop at the designated point, reporting the completion of the mission. This all works, but unfortunately, in AUTO mode, the vehicle overshoots the designated point by about 50 centimeters without slowing down at all before reaching the point. When I send the vehicle to the same point using GUIDED mode, everything works as expected - the vehicle slows down at the right moment and accurately reaches the point.
Is AUTO mode supposed to work this way, and should I use GUIDED mode? In GUIDED mode, unfortunately, I do not get information about reaching the point via Mavlink, but I can work around this by calculating the distance from the target in my program.
NOTE: I made multiple edits below as I perused source code and narrowed possible root cause(s).
Have a look at ATC_ACCEL_MAX, ATC_DECEL_MAX, and WP_ACCEL.
If ATC_DECEL_MAX is unachievable, that is likely the cause (I don’t think GUIDED mode uses this parameter). Reviewing a .bin log would reveal deceleration undershoots.
If WP_ACCEL is non-zero, that could also cause problems.
You might try try making a mission consisting of LOITER_UNLIMITED waypoints. You can use Mission Planner’s Action tab to select each by number (Set WP button), and the vehicle should stop indefinitely at each. I’m also hopeful that it helps keep the navigation controller active for longer, which may help with stopping precision (rather than having the intended hammering point as a simple waypoint that is the last one in sequence).
Using the LOITER_UNLIMITED method, you should be able to view xtrack_error on the Quick tab to monitor the precision of each stop.
Otherwise, I have contacted another developer to help, since there are a few possible causes within the navigation controller.
I also have an idea for a Lua script to use GUIDED mode to navigate waypoints with more finite control over speed. Since we can continuously update the velocity vector, we can slow incrementally as we approach, and potentially stop more precisely (and maybe even back up a few meters and try again, if we miss by an unacceptable margin). This will take a little time to develop, but is a potential path to success if we don’t find a better solution, first.
Wow I am impressed with all the details of your answer! I am excited to test all those suggestions right after the weekend. I will be back with the update. Thanks
Great project. When the vehicle reaches a Guided waypoint it should send a MISSION_ITEM_REACHED message. The only annoying thing is that the Sequence field will always be zero.
Re Guided vs Auto mode navigation. By default Rover’s navigation in Guided mode uses a simpler method which only involves the lower level position controller while Auto mode uses “SCurves” which plans the path before then calling the position controller. You can force Guided mode to use SCurves so that it acts exactly like Auto mode by setting the GUID_OPTIONS parameter to 64 (which is a strange number to have picked but it seemed like a good idea at the time to keep it consistent with Copter).
The downside of using SCurves in Guided mode is that it won’t handle high rate updates to the target position so it really depends upon your use case. If you’re constantly updating the position then it’s better to leave GUID_OPTIONS = 0. If you’re just providing a point and then waiting for it to get there then SCurves might be better. SCurves can handle some updates but just not really fast updates
Today I have tried your suggestions and I have observed the following things:
LOITER_UNLIMITED command gives good effects but it takes a lot of time and I don’t receive MISSION_ITEM_REACHED message.
When using LOITER_LIMIT_TIME I am able to reach the point after some time and I receive MISSION_ITEM_REACHED message.
Still in both of these modes the machine passes the point without slowing down. After that it goes back. It takes a lot of time (over 30 seconds).
The ACCEL parameters are: ATC_ACCEL_MAX = 0.5, ATC_DECEL_MAX = 0, and WP_ACCEL = 0. I have measured the maximum acceleration values when moving in manual mode and observing “gx” and “gy”.
Tomorrow I will try to proceed in GUIDED mode and detect the waypoint achievement by calculating the distance.
Unfortunately I have also problems with GUIDED mode. The machine behaves totally different when I send location through MissionPlanner and through my program. When point is sent by MP the machine moves smoothly making small moves during turning. When I send the same location with my program the machine makes aggressive corrections when going off the course and it does not stop precisely. I have compared the MavLink packets sent by both programs with Wireshark (MavLink plugin) and they are exactly the same.
What do you mean by message rate? Should I send something back to autopilot after I start the GUIDED mode?
I only send HEARTBEAT message in each second. When I want to start movement I do the following:
Set GUIDED mode (MAV_CMD_DO_SET_MODE: Mode = 1, CustomMode = 15)
I think you’re looking at the wrong telemetry figures for acceleration. You should be looking at ax:
Typically, the guided mode target gets updated at something greater than 1Hz. I’m not sure exactly how Mission Planner sends the “Fly to Here” commands.
I messed a little with scripting a solution but didn’t get very far with the time I had to devote. I think setting a POSVEL_NED target could be useful, and you could scale the velocity vector with distance. That was my intended outcome, but I simply ran out of time to get something working reliably today. I will try again later this week.
Of course, the underlying assumption to all of this is that your machine is well tuned for speed, steering, and navigation. If it is not, then we are kind of pissing in the wind.
