@Leonardthall I had a chance to perform takeoffs and landings in loiter mode while testing the z controller relax feature. I’m assuming here that the position controller in 4.2-dev is basically the same as 4.1. I had some issues when taking off and landing in loiter where the aircraft wanted to pitch or roll to follow the target (which I did not move so it was GPS drift). In any case I ended up fighting the loiter controller to keep the rotor head neutral. I’m not so sure what can be done for landing as we are already freezing the position controller once it declares Land_complete_maybe. Here is a plot of that process.
I put the East and North velocities to give a sense of what the position controller was doing. There is a short period of time, close to a half of a second, for the collective to settle below the land_coll_min value to declare land complete maybe. Like I said, I’m not sure there is more we can do here. Maybe procedurally by lowering the collective stick quickly once landed to drive the command higher descent rates on the ground to get the controller to lower the collective quicker. I don’t think we want to linger with the collective stick at low descent rates once landed.
Now for takeoff, I believe we have some improvements that can be made. Here is a plot of a takeoff
The not_landed flag is declared immediately upon entering the takeoff sequence which releases the position controller and the attitude controller integrator terms. This allows a lot of position error and integrator build up before the aircraft is even light on the skids. Why can’t we handle the position error and integrator like we handle the integrator for takeoffs in stabilize mode where the not_landed flag is not declared until the collective/throttle is 50% of the hover collective/throttle? The easiest way would be holding the not_landed flag until the collective/throttle point for stabilize takeoffs but I’m guessing that may have other implications to the code. The soften_for_landing method could be used while taking off until the 50% hover collective/throttle position is reached.
We have a number of issues here we need to take care of to be happy with where we are at with the position controller.
Initial state exceeds our limits. We have a fix in at the moment but we need to ensure it doesn’t interact with our other fixes.
Initial state is sensitive to accel noise. I think we can zero our desired accel and fix this. This is probably the most important fix to go back into 4.1.
Relax is not perfect. Now I suspect this is being made harder to get right due to the first two fixes.
Finally more on topic, take off is sub optimal. As you pointed out one of the main issues is we are running the position controller while still on the ground.
My goal for this week is to fix all these problems and also implement a pause mission command.
On the take off issue. I have been thinking about this and I think the heart of the problem is we do not handle the ground contact portion of the take off separately from the flying portion. Instead we assume the aircraft is flying so quickly we can ignore that portion.
My proposed fix:
I would like to implement a TOTRT “take off throttle rise time” parameter. This parameter would define the time taken to increase the throttle (vertical actuator) from zero to 100%. When the aircraft takes off the throttle is linearly increased from it’s initial value (zero probably) to 100% over that time. At the end of that time we declare the aircraft is flying (land_complete = false). Obviously this will result in the throttle going to maximum on every launch but it guarantees that this phase will always complete after TOTRT. To remove the full throttle take off problem we can add a couple checks such as:
Positive acceleration reaching half accel max
Positive climb rate achieving half vel max
Positive change in altitude of greater than some altitude say WP_NAVALT_MIN or maybe some hard coded value, 0.5m for example.
The idea of these tests would be to change to position controller height control at some time before the aircraft reaches maximum throttle.
So the worst case here is noise causes the change to the position controller immediately resulting something resembling our current take off. Or we reach full throttle before triggering one of our checks, overloaded aircraft maybe, EKF problems, pressure bubble… Not sure.
The idea of this is we are delaying our navigation to immediately after we loose contact with the ground, ie flying, where our control should work as it was intended.
To make this easier I need to remove loiter from the take off procedure so the code is simpler and we can handle the position controller directly.
Part of this problem is the very high noise that helicopters get as soon as they touch the ground.
In your case you get accelerations of ± 3 or 4 m/s^2 at 50 Hz.
The accel filter INS_ACCEL_FILTER is set to 20 Hz so this is letting much more noise through than we would like. So this is part of the problem. Dropping the accel filter to 5 Hz would improve this significantly.
This is solved during take off using the throttle ramp approach. During landing it is fine as we land because we saturate the collective in the low position. As soon as we relax the controller we remove this saturation. Further complicating matters is these accelerations exceed your maximum acceleration meaning if we limit our initialisation values we can’t zero the controllers.
Relax I terms is supposed to slew the controller back to zero or some predetermined value.
Relax controller is supposed to slew the output of the controller to some predetermined value.
We relax the controller by:
Set the position target to the current position - correction velocity is zero
Set the desired velocity to the current velocity - target velocity is correction velocity + desired velocity
Target velocity is equal to the current velocity so correction acceleration is zero.
Set the desired acceleration to the current acceleration and limit to acceleration max - target acceleration is correction correction + desired acceleration.
reset acceleration PID filter to zero error and D term.
So we are left with zero output from the PD terms of the accel controller leaving only the I term.
