Copter: improve shaping of rate controlled axes

[bnsgeyer]

This PR improves the shaping for rate controlled axes. This would include the yaw axis in stabilize mode and pitch, roll and yaw axes in Acro mode. The target rate shaping is improved by using the square root controller to provide a first order response to achieve the requested rate. Currently this is done by linearly increasing rate at ACCEL_MAX until the requested rate is achieved. Using the ACCEL_MAX to adjust the shape is not conducive to shaping the response. It doesn't adjust the acceleration request with the size of the rate request. The square root controller does a nice job at that.

This adds two parameters to allow users to adjust the rise time of the target rate to achieve the requested rate. One for pitch and roll axes and the other for yaw axis.

[rmackay9]

We discussed this on the dev call and @bnsgeyer is going to move the parameters to the vehicle code.

[bnsgeyer]

@rmackay9 need your help with how to pass in the time constants. I took my best guess at doing this. Please review and let me know what you think. It seems like a lot to pass them in on every cycle. I thought that we might be able to get away with passing them in when you enter the flight mode through the INIT function. However if someone was to make a change to a parameter while in the flight mode it would never be seen by the attitude controller. Looking for guidance.

[lthall]

Looks great to me.

[bnsgeyer]

@lthall @rmackay9 So I did as Randy suggested and moved the time constant into the shaping call instead of having it as a separate accessor set function. It made me realize what I probably missed as far as parts of the code that use these shaping functions. So this change I just made was very invasive and will affect both plane and sub. I designed it that if you put zeros in for the time constants, it will revert back to the existing shaping function. But everywhere that calls any of these shaping functions will need to have the arguments added to comply with the new list.

I thought of making a separate shaping function with the extra arguments that would be used only by copter and that would keep from having to mess with plane and sub. the function would just set the time constants and then call the existing function. Thoughts?

[peterbarker]

 Binary Name      Text [B]         Data [B]     BSS (B)        Total Flash Change [B] (%)      Flash Free After PR (B)
---------------  ---------------  -----------  -------------  ----------------------------  -------------------------
antennatracker   0 (0.0000%)      0 (0.0000%)  0 (0.0000%)    0 (0.0000%)                                      320792
blimp            0 (0.0000%)      0 (0.0000%)  0 (0.0000%)    0 (0.0000%)                                      302320
ardurover        0 (0.0000%)      0 (0.0000%)  0 (0.0000%)    0 (0.0000%)                                      142328
arducopter-heli  1644 (+0.1745%)  0 (0.0000%)  -4 (-0.0031%)  1644 (+0.1741%)                                   36988
ardusub          880 (+0.1085%)   0 (0.0000%)  0 (0.0000%)    880 (+0.1084%)                                   170160
arduplane        868 (+0.0898%)   0 (0.0000%)  4 (+0.0031%)   868 (+0.0896%)                                    13120
arducopter       1476 (+0.1588%)  0 (0.0000%)  -4 (-0.0031%)  1476 (+0.1585%)                                   50116

[rmackay9]

We discussed this on the dev call today and agreed the approach looks good and BillG is going to add in the required param conversion. The enhancement to quadplanes to use this can be done in a future PR (PeterH may have expressed some interest in doing this).

[rmackay9]

This looks good to me. Just a few things and then we're OK to merge

1. add const to some get methods
2. remove the AP_Floats
3. test the case where the param is zero

BillG, Tridge and IamPete1 suggested that if this command model was used in Plane and Sub we could reclaim about 1K of flash.

[bnsgeyer]

@rmackay9 So I was able to combine the two methods into input_shaping_ang_vel putting the if statement on rate_tc in the method. Much cleaner and really nothing needs to be done once quadplane adds the command model params.
I also did what you asked above

add const to some get methods
remove the AP_Floats
test the case where the param is zero

lastly I set the default for the rate_tc to zero so there will be no change in behavior. I tested to ensure the rates were being shaped properly with the time constant and without and flew it in real flight as one final sanity check.

please review and merge if you are happy.

Thanks,
Bill

[rmackay9]

Merged, thanks!

[rmackay9]

I haven't included this in Copter-4.2.3-rc1 because it's a big enough change to make me worry and so I'd prefer we leave it to 4.3 which starts beta testing in a few weeks. Please tell me if you strongly disagree though because it's a team effort after all.

[bnsgeyer]

@rmackay9 thats what I expected, I don’t think there is any reason that it has to go into 4.2. Thanks!