Copter: surface tracking fix

[lthall]

This PR updates the surface tracking code to use the new terrain following algorithms in the position controller.

The existing code relies on the slower response of the target following approach and now causes overshoot as seen in this report:
https://discuss.ardupilot.org/t/altitude-overshoot-on-ascent-descent-tuning-help-please/74756

[rmackay9]

I'm put some suggestions for non-functional changes but in general this is looking pretty good to me.

[rmackay9]

I've tested this and it seems to be working well.

[IamPete1]

I think we can keep most of the removed consts. Looks like making

ardupilot/libraries/AP_Math/control.cpp

Line 32 in 8462ce4

void update_vel_accel(float& vel, float accel, float dt, float limit)

void update_vel_accel(float& vel, const float accel, const float dt, const float limit)

Would allow the consts to remain.

Looks like we can also remove RNGFND_GAIN param as a result of this.

[rmackay9]

@IamPete1,

Thanks very much for the review. Removing RNGFND_GAIN is a bit historic I think.

We don't need any of those consts. There was one outright mistake where it was using a const reference ("const float &") but never changing the value and the rest are unnecessary const-ing of pass-by-value arguments.

[rmackay9]

@IamPete1, fixed, this resolves your comments i guess? Thanks again for the feedback on this complex PR

[tatsuy]

Thank you for this improvement.
There are no problem with normal flight.

When I use upward facing terrain following, DSAlt shows negative value.
And it is sometimes intermittent.

[lthall]

@tatsuy If you are using upward facing You should be below it right? So it should read negative.

Can you send me this log? Maybe make it available on the discuss forums?

[IamPete1]

Suggested change

	TUNING_RANGEFINDER_GAIN = 41, // unused
	TUNING_EKF_VERTICAL_POS = 42, // unused
	TUNING_EKF_HORIZONTAL_POS = 43, // unused
	TUNING_EKF_ACCEL_NOISE = 44, // unused

If we just delete all of these then we can be sure nowhere in the code is trying to use them.

the param values need a update here:

ardupilot/ArduCopter/Parameters.cpp

Line 349 in 9d32e48

// @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,58:SysID Magnitude

[lthall]

Good catch on the Parameter description!
I have not removed the lines in the defines.h file as that is a style change and best off left as a separate commit from people with style.

[tatsuy]

@lthall Previously, it was displayed as positive values.
Current master

This branch

this is the log.
SurfaceTrackingFix.zip
I've changed a bit to test this in SITL. and then I enabled Proximity avoidance.
https://github.com/tatsuy/ardupilot/commits/20210831_SurfaceTrackingFix

About being intermittent, sorry, this is my mistake.
I checked again, and there was no problem.

[lthall]

@tatsuy, I see what you are saying. I will chat to Randy about this.

Does everything work with the upwards facing lidar even though the log is backwards compared to what it was before?

[tatsuy]

@lthall

Does everything work with the upwards facing lidar even though the log is backwards compared to what it was before?

yes,it works with upward facing terrain following and proximity

[lthall]

@tatsuy I have reversed the log so it should match the old behaviour now. Thanks for highlighting this. Could you take another look?

[tatsuy]

@lthall Everything is working fine for me. Thank you!

removed the extra param so dismissin PEter's review, thanks!

[tatsuy]

@rmackay9 Can we add this to Copter-4.1?

[rmackay9]

@tatsuy, yes, this will be included in the next beta release hopefully later this week. Txs very much for uncovering that issue!

[tatsuy]

Great, thank you.