I saw in a similar topic where there is interest in having mowing patterns reverse at the ends. @Yuri_Rage proposed a method to do this by setting DO_SET_REVERSE at alternate passes.
I thought about it and it dawned on me perhaps an elegant solution to avoid pivot turns is to detect acute turning angles in the waypoints and automatically insert alternate reversing commands. The attached plugin does just this.
It should work with any mowing pattern too. As a precaution it adds a DO_SET_REVERSE = 0 at the beginning and end of the mission to make sure the mower is traveling forward at the start and end.
I’ve only had a chance to run it through the simulator but so far it seems to work pretty well. Give it a try and let me know how it works for you. I think it can be a powerful tool for mowing ops.
To use: download into your plugins directory, rename to reverser.cs and start mission planner. You will have a new right click menu item in the planner. Right click and select Reverse and it will insert the reversing commands. You can do it repeatedly as it replaces any previous reversing commands with new ones.
You assume all risk using this!
I have since added one small improvement which is the ability to remove the reversing commands. Attached is the updated plugin code. reverser.txt (5.8 KB)
Interesting, that’s not my experience. I just put the .cs file in the C:\Program Files (x86)\Mission Planner\plugins folder and MP picks it up, compiles and adds to the plugins listing (Ctrl + P).
Here is the response. It went from 155 lines to 115. Unsurprisingly it didn’t work as it removed a needed library, this I fixed. However many of the simplifications were definite improvements.
using System;
using System.Linq;
using System.Windows.Forms;
using MissionPlanner;
using MissionPlanner.Controls;
using MissionPlanner.Utilities;
namespace Shortcuts
{
public class Plugin : MissionPlanner.Plugin.Plugin
{
ToolStripMenuItem reverserMenu;
MyDataGridView commands;
public override string Name => "Waypoint Reverser Plugin";
public override string Version => "0.11";
public override string Author => "John Harlow";
public override bool Init() => true;
public override bool Loaded()
{
reverserMenu = new ToolStripMenuItem("Reverser");
reverserMenu.DropDownItems.Add("Add Reversing commands", null, AddReversingCommands);
reverserMenu.DropDownItems.Add("Remove Reversing commands", null, RemoveReversingCommands);
Host.FPMenuMap.Items.Insert(0, reverserMenu);
commands = Host.MainForm.FlightPlanner.Controls.Find("Commands", true)
.FirstOrDefault() as MyDataGridView;
return true;
}
public override bool Loop() => true;
public override bool Exit() => true;
void AddReversingCommands(object sender, EventArgs e)
{
RemoveReversingCommands(sender, e);
// Insert initial reverse command
InsertReverseCommand(0, 0);
DataGridViewRow prev2 = null, prev1 = null;
bool reverseDir = true;
for (int i = 1; i < commands.Rows.Count; i++)
{
var row = commands.Rows[i];
if (IsWaypoint(row))
{
if (prev2 != null && prev1 != null)
{
double ang = GetAngleBetween(prev2, prev1, row);
if (ang > 90)
{
InsertReverseCommand(i, reverseDir ? 1 : 0);
reverseDir = !reverseDir;
i++;
}
}
prev2 = prev1;
prev1 = row;
}
}
InsertReverseCommand(commands.Rows.Count, 0);
Host.MainForm.FlightPlanner.writeKML();
}
void RemoveReversingCommands(object sender, EventArgs e)
{
for (int i = 0; i < commands.Rows.Count; i++)
{
if (IsReverseCommand(commands.Rows[i]))
{
commands.Rows.RemoveAt(i);
i--;
}
}
Host.MainForm.FlightPlanner.writeKML();
}
bool IsWaypoint(DataGridViewRow row) =>
row.Cells[0].Value?.ToString() == MAVLink.MAV_CMD.WAYPOINT.ToString();
bool IsReverseCommand(DataGridViewRow row) =>
row.Cells[0].Value?.ToString() == MAVLink.MAV_CMD.DO_SET_REVERSE.ToString();
void InsertReverseCommand(int index, int value)
{
var cmd = new DataGridViewRow();
cmd.CreateCells(commands);
cmd.Cells[0].Value = MAVLink.MAV_CMD.DO_SET_REVERSE.ToString();
cmd.Cells[1].Value = value;
commands.Rows.Insert(index, cmd);
}
double GetAngleBetween(DataGridViewRow a, DataGridViewRow b, DataGridViewRow c)
{
double lat1 = double.Parse(a.Cells[5].Value.ToString());
double lon1 = double.Parse(a.Cells[6].Value.ToString());
var p1 = new PointLatLngAlt(lat1, lon1, 0, null);
double lat2 = double.Parse(b.Cells[5].Value.ToString());
double lon2 = double.Parse(b.Cells[6].Value.ToString());
var p2 = new PointLatLngAlt(lat2, lon2, 0, null);
double lat3 = double.Parse(c.Cells[5].Value.ToString());
double lon3 = double.Parse(c.Cells[6].Value.ToString());
var p3 = new PointLatLngAlt(lat3, lon3, 0, null);
double bearing = p1.GetBearing(p2);
return Math.Abs(p2.GetAngle(p3, bearing));
}
}
}
Finally had a chance to test it on the mower. Not bad for a first go.
It has some odd dynamics at the reversing transitions. My theory is the EKF might take a second to react to all the values going negative. Or something. Not sure it is designed to work this way.
Hey John - this is excellent ! thanks for sharing.
I threw a quick mission at it in the simulator (skid-steer rover) and it works a treat.
I’ve been meaning to figure this out for some time - but my coding skills mean any time savings on the mow is negated by slow coding progress…
Once i get the mower back in the grass i’ll try it out for real and report back. Our tracked mower leaves big squid tenticle marks everytime it rotates (on top of the previous spot) so lawns with spots will be less of a thing with just a simple click after the plan. Wide tracked mowers turn slowly, so this optimisation will save time also. I’ll compare a couple of simulations on our property and post the difference (time permitting)
Just thinking around - every mower is different, we have headlights and bumper bar on the front, but yet to wire up the back bumper, so forwards is slightly preferable to backwards. I’ll poke around and optimise for forward travel on the long straights, and do some testing with the 90degree rule, ie only reverse if ang>100 etc.
i like your GetAngleBetween - very cool.
Desktop testing is awesome with the simulator (thanks SITL team)
Thanks for the reply. Agree this might prove to be most beneficial to tracked vehicles as they can seriously damage turf during pivot turns. Reversing ought to be faster but in my test the AP seems to have a problem figuring out how to get back on track, so to speak, as can be seen in the video. Hopefully the solution is within one of the seemingly endless tweakable parameters.
And yes the mower running in reverse does present new potential issues; particularly in regards to forward facing obstacle avoidance devices and cameras. I have a front facing camera on mine and when in reverse shows a great view of where it went. Not good. I’m thinking about making a servo mount for it and having it reverse the camera as well. It’d be another simple command insertion for the plugin fortunately.
Please keep me posted on your testing. I really only just started messing with and would think this mowing style has a lot of promise.
I will try it on my “workhorse” gas mower when I get a chance.
