Rover S-Curves alpha testing

[rmackay9]

@Yuri_Rage,

Great, thanks for the feedback.

Re the waypoint mission disappearing, I tested it on my CubeOrange and couldn’t reproduce the problem. I suspect it is either the GCS or a script that is erasing the mission. If you’re using QGC, I know it has an option to delete the mission but I don’t have many details of this feature. Maybe try testing a couple of reboots with MP? In case it’s the script could you try setting SCR_ENABLE = 0 and then try rebooting, re-uploading, re-booting etc to see if the problem persists?

Re the delay at waypoints I can imagine a few possible causes:

1. the pivot turn is too strict in enforcing the angle target
2. the pivot turn does not start until the vehicle has stopped (previously it would start the moment the vehicle passed through the WP_RADIUS)
3. the SCurve jerk (change in acceleration) is too low. To avoid overloading people with parameters I’ve hard-coded the jerk to 1m/s/s/s. This will make the vehicle’s slow down before the pivot and speed up after the pivot slower than in master.

I suspect the issue is probably 3… I’ll add a jerk parameter and we can play with that to see if it helps. I might also add some debug output so we can easily see what stage of the turn the vehicle is at and where the time is going.

Txs again!

[rmackay9]

@Yuri_Rage,

I’ve updated the links to the binaries at the top of this discussion. This new version (rover-42dev-scurves-xxx-24Dec2021.apj) includes these changes to help get to the bottom of the delays at corners:

1. “Pivot to xxx started” and “Pivot complete” messages to help clarify where the delay in the pivot is happening (e.g. before the pivot, during the pivot or after the pivot)
2. reduced the WP_PIVOT_DELAY default to 0 (which won’t affect you because you’ve already set it to 0)
3. added WP_ACCEL and WP_JERK parameters to allow configuration of the SCurve accel and jerk in autonomous modes separate from manual modes. Increasing these values should reduce any delay seen before or after pivot turns.
   • if WP_ACCEL is set to zero (the default) then the ATC_ACCEL value will be used (i.e. no change in behaviour)
   • if WP_JERK is set to zero (the default) then the WP_ACCEL or ATC_ACCEL value is used for jerk (i.e. no change in behaviour from previous test version).

… I can actually imagine that even if you increase the WP_ACCEL and WP_JERK parameter values you’ll still see a delay before/after the pivot turns in which case it means the SCurves have a built in delay which I will need to discuss with @Leonardthall.

[Yuri_Rage]

@rmackay9, thank you for the updated binary!

I am still experiencing the loss of mission waypoints between reboots, even with scripting disabled and MIS_OPTIONS=0. I always use Mission Planner for GCS software.

The “Pivot to x started” message is repeated for the duration of the pivot turn, which makes it a little difficult to decipher the message timing, so I actually rebuilt your rover-scurve5 branch with a couple of minor debug edits as follows, including a report of the pivot delay value:

bool _did_report_pivot_start = false;
void AR_PivotTurn::check_activation(float desired_heading_deg, bool force_active)
...
        if (!_did_report_pivot_start) {
            gcs().send_text(MAV_SEVERITY_CRITICAL, "Pivot to %d started", (int)desired_heading_deg);
            _did_report_pivot_start = true;
        }
...
        gcs().send_text(MAV_SEVERITY_CRITICAL, "Pivot complete after %dms delay", (int)get_delay_duration_ms());
        _did_report_pivot_start = false;

Indeed, the reported delay value is 0 unless commanded otherwise by parameter changes.

The delay is happening BEFORE the pivot turn. I think it has to do with the determination of whether the waypoint has been reached because I also note a similar delay at the end of the mission when the mower stops - the “mission complete” message has about the same amount of delay. There is also a brief pause between most non-pivoted waypoints.

I tried increasing WP_JERK and/or WP_ACCEL using values between 1 and 5, and that made little observable difference.

I also tried WP_RADIUS values between 0 and 5 with no observable difference.

And I tried a number of throttle and steering accel, rate, and PSC_VEL* values, but none of them made any positive difference, and I returned them to roughly the same tune as I achieved via ACRO mode observations.

I spent today “playing robots” (as my family calls it) for a bit, cooking, and enjoying the holiday with family. S-Curve support for Rover almost feels like a Christmas gift. Merry Christmas, Happy Holidays, and all the best to you and the ArduPilot community!

[rmackay9]

@Yuri_Rage,

Thanks very much for that testing and bug fix. I’ve compiled a new version which includes an equivalent fix and also adds an “scurve finished” message when the scurves portion complete (before the pivot).

When you have a chance I’d like to get a log from you to see if you’re seeing what I’m seeing in SITL. This could be with the latest binary i’ve linked in the top discussion or just from this most recent test you’ve done.

… basically I can reproduce the delay in SITL so I should be able to get to the bottom of it.

[Yuri_Rage]

Sounds good! I’ll try and get a test session in tomorrow before it rains this week.

EDIT:
You should have Google Drive access to my latest test session log from Christmas Eve.

[rmackay9]

Hi @Yuri_Rage,

Thanks for that. I can’t immediately find the link to this latest log (I’ve got one for the previous log).

In any case, this delay at the waypoint is caused by a slight overshoot in the speed controller while stopping (it stops too quickly) which has the knock-on effect of causing the SCurve “time scaling” code to slow the SCurves progress. FYI this “time scaling” code is there to deal with situations where the vehicle can’t keep up with the pre-calculated path. It basically slows down the path so the vehicle can catch-up.

If you could reduce CRUISE_THROTTLE from “55” to “50” or maybe even “45” that might reduce the overshoot and the problem might go away.

… in the meantime Leonard suggests we should turn off the time scaling during the braking phase so I will come up with a new version soon which does that.

Txs again for your help with this.

[Yuri_Rage]

Well, it just got dark, but as luck would have it, I want to test the new SIYI camera in low light (update - it worked fairly well with some supplemental lighting). I will load your latest binary with the message fixes, reduce the throttle setting, and send a log your way shortly.

[Yuri_Rage]

@rmackay9, I just uploaded a 20211230 .bin log to a Google Drive folder and gave you complete access to that folder using your Yahoo email address.

I don’t think the lower CRUISE_THROTTLE setting made much difference.

[John_Easton]

Just spotted this topic now, brilliant stuff.

Please note that at 55 years of age I have ZERO dev skills, and very likely will never change that. However, I am firstly a boat skipper and secondly a cartographer. On these two things I have many years of experience and am more than happy to share where I can.

When I started using autonomous craft for collecting sonar data in late 2015 one of the single biggest struggles I ran into was ‘Wobbly Path between Waypoints’.

Lesson 1 - Throttle
In 2019 I accidentally got my boat to go straight from waypoint to waypoint when the throttle lever came loose and the craft maintained a 100% steady throttle which resulted in my very first perfect grid. The lesson learned here is that it was the varying throttle that was the culprit.

Lesson 2 - Size & Weight of Craft
Just when I thought I had solved my problems, each boat was different. To date I cannot get the same navigation performance on my other craft even if it is identical in size and shape, but just heavier. The lesson here is don’t expect an ‘oil tanker’ to turn like a ‘speed boat’.

Lesson 3 - Turning Capability
No two craft are identical, even if they look the same. This will effect your navigation performance profoundly. Even a full fuel tank at the start of the day compared to empty at the end of the day is going to make a difference to the turning capability. So first step for any craft before doing anything is to determine the Turning Capability to both port and starboard if you using an outboard type motor due to the torque of the prop in the water. Weather and current conditions can also influence this in a big way.
Here is what I get on my 4m AIMy -

Lesson 4 - Heading Angles WPT to WPT
As long as there are no parameters ‘fiddling’ with the throttle or steering in Auto, we know that my 4m AIMy for example can 100% make a turn with a radius of 8,5m to both port and starboard weather permitting.
Therefore if she were to start a turn 8,5m from the waypoint on a right angled turn she would exit perfectly on the track to the next waypoint and there would be no need for overshoot or oversteer.

Should the angle be acute, then at a 60° angle for example she would need to start turning around 12m before the waypoint.

[Yuri_Rage]

I think this evolution will help you quite a bit. The tuning is more straightforward, and the position controller does a nice job of nailing the course and heading with less fiddling overall. There are some kinks to work out, and I’m eternally grateful to Randy, Dr Hall, and the entire dev team for their efforts. Happy New Year!

[John_Easton]

Yuri I agree 100% on both the subject as well as those making it all happen.

[rmackay9]

I’ve update the binaries linked from the top description with the following changes:

1. SCurve time scaling disabled during braking period before stopping at a waypoint (this should reduce the delay @Yuri_Rage has been seeing before pivot turns). Also added WPDT debug log message so we can more easily see what’s going on.
2. Stop or loiter at waypoints which have a delay (previously the position controller remained active during the delay which could cause the vehicle to rock back and forth)
3. Guided mode and BendyRuler may pivot if required
4. Guided mode reports when it has reached destination
5. Internal Errors added if invalid destinations received in Auto, Guide, RTL, etc (should never happen)
6. Rebased on master
7. CRUISE_SPEED and CRUISE_THROTTLE are used to set the ATC_SPEED_FF value (this should not affect performance but does allow us to see the speed controller’s feed forward output in the logs)

All feedback is greatly appreciated!

[Yuri_Rage]

Well, I can report that the changes are significant! But…not as impactful as I had hoped.

As you may recall, I had previously increased the ATC_STR_ANG_P and ATC_STR_RAT_FF parameters to be significantly more aggressive than under L1 control. With this latest revision, I had to dial those back closer to their previous settings, lest the mower wildly overshoot. So, something has changed!

I also tried enabling one fence and BendyRuler avoidance. I think that was too much to ask given the state of the tune. I don’t think I trust things enough to enable that again under this revision. If we can get things behaving a little more predictably, I can (and will) try that again.

However, I’m still seeing delays as bad or even worse than previous. I have a few things to do before testing further, but I will grab a log from a clean boot cycle later this evening.

AND - I just can’t seem to figure out how to keep from wiping the waypoints out between reboots. With scripting disabled and all parameters set to oppose deleting waypoints, they still disappear after rebooting the autopilot.

@rmackay9, log uploaded to the same Google Drive folder I’ve been using. Upon review of the log, there are quite a few steering and throttle overshoots, so I may still have the controller inputs a little too aggressively tuned, but I hope the log is of value, nonetheless.

[rmackay9]

Hi @Yuri_Rage,

Thanks for giving it a try. I wasn’t expecting you to find any significant change in behaviour besides a slight improvement in the delay before pivot turns so we will need to get to the bottom of this.

The previous version, rover-42dev-scurves-cubeorange-27Dec2021, is still available here

I guess the issue could be a change in the new SCurve feature, a change to master, an unintended parameter change or some physical change in the vehicle.

I’ve done a fair bit of testing in the simulator but I’ll put this on my AION robotics rover and see how it does and maybe I can uncover what’s happened.

Txs again.

[Yuri_Rage]

No worries, happy to help. I have a small Ackerman-steer truck that I never tuned well. I was considering loading one of these S-Curve variants onto it and attempting a tune for comparison.

[rmackay9]

@Yuri_Rage,

By the way, for Ackerman steering vehicles I hope to improve their performance in the future (not as part of this change) by adding an equivalent to the skid-steering vehicle’s pivot turn controller.

Below is a picture from the simulator in which the Ackermann vehicle was initially stopped at waypoint 1 (the corner) and then continued to the next waypoint off to the West. The yellow dots show that because the vehicle cannot physically move laterally we end up with I-term build up in the position controller which leads to overshoot below the line. We can fix this by adding a turn controller that plans the turn.

[rmackay9]

@Yuri_Rage,

Just two other small things. Could you try master to see if the mission wipe problem occurs? If it doesn’t occur with master then it must surely be related to the parameter conversion code I added somehow affecting the MIS_TOTAL parameter. If it does still occur with master then we’ve got a much wider problem and I might ask another developer to help me get to the cause.

I forgot to mention that I changed the speed controller to set the ATC_SPEED_FF value from the CRUISE_SPEED and CRUISE_THROTTLE values. This shouldn’t have any effect but maybe I messed up.

[Yuri_Rage]

I can definitely do that to check for waypoint disappearance.

I’ve been changing a lot of tune parameters throughout this process, aiming for the best results. This last change caught me off guard, and I probably did not tune as well as I could have, given the drastic change in behavior.

[Yuri_Rage]

@rmackay9, waypoint issue solved! It was all my fault. The SD card was full and beginning to overwrite log files, and I guess current waypoints are discarded once that begins to happen.

Once I freed some space, everything was back to normal ops. I hope you spent little to no time investigating my own error!

[rmackay9]

@Yuri_Rage,

Great! That is very interesting though… I really wouldn’t expect a full SD card to have that effect because on most boards (including the cubes) the missions are stored in eeprom along with the parameters. I’ll raise an issue and chat with @peterbarker about this to see if he has ideas.