There was an error in the velocity axis used to update
brake_timeout_pitch (vel_right instead of vel_fw)
The wind_comp was not enough filtered for the Pixhawk (400Hz), so I
added a specific time constant (TC_WIND_COMP) to have the expected
filter with 400Hz controllers.
About throttle peaks, after some tests and from logs, they happen when
hybrid switches to loiter.
There is always a difference between Alt and DesiredAlt (DAlt), but,
when loiter engages, it initializes DAlt = Alt and the copter tries
immediatelly to reach that new setpoint. So the solution would be to
init_loiter_target() just as it was in pre-onion code : only x/y and not
z. and to be able to pass parameters like that
wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
Well, from this new code structure, it seems not possible with current
functions so I've used set_loiter_target that init position passed as
parameter and velocity to 0 (as expected).
BTW, I think there was something wrong with set_loiter_target function,
the "Vector3f& position" parameter was not used at all...
I moved the reset flag from init_loiter_target to set_loiter_target.
Corrected timer init values and wind_comp time constant to get the best
behaviour
Check if wider deadband required (current is 30).
We were using a comparison to a 70-100 cdeg deadband in our initial
code. The deadband was there to avoid unwanted switches in case of
inaccurate fingers/radio
Adjusted timers to 100/400Hz
I_term does not init when in hybrid loiter
this will return 0 when there is no current command, which is what is
expected in MAVLink when there is no mission
(it prevents the text to speech announcing "65 thousand 6 hundred and
thirty five", which is quite annoying!)
A magnetometer axis that fails the innovation consistency check will cause
all axes not to be used. If this condition continues for 10 seconds, a
magnetometer timeout condition will be declared. When the timeout has
occurred, if it is not a fly forward vehicle, then individual channels
will be used again, but with a reduced weighting.
