Heli: Add blade pitch angle logging

[MattKear]

This pr adds logging for blade pitch angle contributions from collective and cyclic. Whilst it is possible to calculate these values from other log fields it requires the user to understand how we do the blade angle conversions and to know how the mixers work. This is therefore a convenient logging field.

This PR only makes changes to logging. There are no functional changes to flight dynamics or actuators.
For cyclic angle contributions to be accurate users will need to set the newly added H_CYC_MAX_ANG param.

Example from dual heli logging in SITL:

[MattKear]

Updated following a few suggestions on discord.

@IamPete1 I have done your suggestion for instance logging per swashplate. This is a nice improvement, good idea!

I looked at moving the logging into the swashplate class as per your suggestion. It really should be there, however, all of the collective angle and range values are in the heli motors lib, which we do not inherit from. The two implimentations I tried (passing values down to swash and passing motors pointer down to swash) just ended up looking a mess and being a bit confusing to follow, hence I have opted for this implementation. I think the best way forward in the future would be to move the collective params and scaling calculations down into the swashplate lib. The logging can then be moved at the same time. Trying to keep it sperate for now to make it easier to review. This PR is logging only and I think I have done it in such away that it can easily be moved into swashplate when we are ready.

@bnsgeyer I have removed the cyclic angle max param and inferred the output to blade angle from the collective params as you suggested. Boy was that a head scratcher!! Great idea though.

I have done plenty of testing on single heli, but do not have access to dual or quad hardware at the moment. I am confident they are correct though.

First I tested the max/min angles and how they vary with H_CYC_MAX. Zeroing the blade pitch gauge on zero pitch and then hunting around the roll/pitch stick to find max and min angles:

I did this for a range of values for H_CYC_MAX and compared to the calculated value from this implementation:

I then did a test of the angles measured vs calculated for a range of stick positions with a fixed H_CYC_MAX. To make this achievable I had to position the blades in an azimuth such that max cyclic could be achieved in one axis (pitch in this case) so that I could use the springs in the transmitter stick to help keep my hand steady:

Given that the pitch gauge I am using for the measurement is not what I would call a "quality instrument" 😆, there will be some non-linearity in the servos, and some marginal discrepancies in angles from my collective/pitch link setup, I think these are pretty good results.

What was interesting was that doing this highlighted to me that we actually do the cyclic scaling slightly differently between dual and single frames. This is why we have to pass the sqrt(2.0) scale factor into the calculation for single. I would like to bring single heli into line with dual by rejigging the scaling. That is a future PR and will allow us to remove that scale factor in the logging.

Tested dual heli in SITL to confirm that the two swashplate instances are logged and have different values:

P.S. I think our dual heli SITL model needs a bit of a tune up 😆

[bnsgeyer]

@Gone4Dirt I took a closer look at the PR and what was being logged. Why are we just logging total cyclic? It makes more sense, IMHO, to log pitch and roll cyclic. I'm not sure what meaning total cyclic would be unless you know what azimuth angle it occurs. Am I missing something?

[MattKear]

@bnsgeyer yes fair comment, I can see why users would want the roll and pitch cyclic contribution. I can add these.

I still want to keep total cyclic contribution though. To explain: Currently we do not log the result of the scaling that we do with the H_CYC_MAX param. So we cannot see if we are saturating on that limit within the controller.

The plots below show a surface that replicates the H_CYC_MAX scaling done in heli single to all combinations of the output from the roll and pitch rate controllers. Roll and Pitch here are the same as the RATE.ROut and RATE.POut log fields. The bit of math I have added in this PR is included to convert the scaled output from the cyclic scaling to a cyclic angle.

What this plot illustrates is that we actually saturate on a H_CYC_MAX when we have not saturated on either roll or pitch rate outputs. Similarly we may not be saturating on a servo output as well. So we are hitting a control output constrain limit but have no easy way to tell in the log.

The fact that I am reporting this as an angle is just because I think it is more intuitive for the user (or at least it is for me). We tell users to tweak the H_CYC_MAX param until they achieve some cyclic angle that they want. So they are already aware of the maximum total cyclic output angle, and this log field relates to that.

[bnsgeyer]

I still want to keep total cyclic contribution though.

@Gone4Dirt I am fine with that.

What this plot illustrates is that we actually saturate on a H_CYC_MAX when we have not saturated on either roll or pitch rate outputs.

Yes, I had noticed that. Just wasn’t sure how to fix it because changing it so that the limits of pitch and roll input are at the H_CYC_MAX would break everyone’s setup. We would have to experiment to ensure that we would be able to convert every users gains so their tune doesn’t change because of this. For now though, I am happy with what you are doing here.

[MattKear]

Added roll and pitch cyclic.
Moved the logging call to be inside of log_write_heli() instead of a separate function.
Re-tested single heli. Outputs appear sensible and match measurements from blade pitch gauge. Logged values from going "around the box" in passthrough:

[MattKear]

Rebased

[IamPete1]

I don't think we need to log Heli_Quad, one can look at the servo outputs directly since there is no mixing.

[MattKear]

I have done as @IamPete1 suggested and I am much happier with the PR as a result.

Most of the logging has been pushed into the swashplate lib. We still have the issue of the collective params being a level higher, but I have done my best to make the implementation fairly clean, passing in a calculated scaler and two param values to the swashplate function.

This also makes use of the motors Log_Write() virtual function which is called at 10 Hz (same as I had it before). This is switched on/off with the LOG_BITMASK 17th bit, currently labelled "Motors" which I think is acceptable. The end result is that there are no Copter level changes.

I have retested the logged values against a blade pitch gauge on my heli to make sure the math is still correct for single heli. I have re-tested dual in SITL to see that both swashplate instances are logged and sensible values.

As per Pete's suggestion, I have removed the heli quad bit that I was originally trying to add.

[MattKear]

When reviewing flight test log I spotted a bug that crept in one of the reworks. Swashplate receives the scaled collective value so to calculate the correct blade angle contribution I have had to reverse that scaling.

Test in SITL to confirm maths is correct.
For both tests H_COL_ANG_MIN = -2 and H_COL_ANG_MAX = 12
Tested with H_SV_MAN = 1 (Pass through)

Single Heli:

Dual Heli:

[MattKear]

From my point of view this PR is ready to go in. It has had a fair bit of real world testing on single heli and maths has been confirmed for dual heli in SITL.
@bnsgeyer what do you think?

[IamPete1]

LGTM.

[bnsgeyer]

LGTM

[bnsgeyer]

Merged! Thanks!