AP_Math: add sanity check for constrain_value parameters

[peterbarker]

We have some code which can call constrain_float with the parameters around the wrong way - and the method isn't really set up for that.

This will raise an error.

Alternatively we could rename "low" and "high" to "boundary1" and "boundary2" and magically switch them around depending on which is higher.

[rmackay9]

Thanks for this. In this case I think we want the existing behaviour or else bendy rule will look beyond the distance to the next waypoint in the rare case that it is very close.

[peterbarker]

Thanks for this. In this case I think we want the existing behaviour or else bendy rule will look beyond the distance to the next waypoint in the rare case that it is very close.

I'll need to work through that - work out what the current behaviour in master actually is!

[peterbarker]

Hmmm. Seem to have lost the fix for the Polyfence avoidance code which gets the constraints around the wrong way....

@rmackay9 I vaguely recall you having thoughts on that one?