fw_tecs: Support tighter altitude tracking during low-height flight

[oravla5]

Solved Problem

Slow TECS reaction to altitude errors during low-height flight could lead to collisions with trees or other high obstacles in the flight path.

Solution

The new parameter FW_T_THR_LOW_HGT allows to define a height threshold under which the TECS altitude tracking becomes tighter according to the already existing parameter FW_LND_THRTC_SC, which reduces the TECS time constant.

Changelog Entry

For release notes:

Feature: Improved altitude tracking during low height flight.
New parameter: FW_T_THR_LOW_HGT
Documentation: See `src/modules/fw_pos_control_li/FixedwingPositionControl.hpp`

Testing

VTOL test flight log

[RomanBapst]

@oravla5 Something that is not so nice about this is that the landing logic also sets the time contstant and it's a separate path that does not use the smoothed version. Maybe we can still look at improving this.

[Jaeyoung-Lim]

@oravla5 IMHO, While TECS tuning might be part of the picture, tighter altitude tracking navigating in terrain needs to be implemented via glide slope tracking.

[RomanBapst]

@oravla5 I agree with @Jaeyoung-Lim on the point that the global state is not ideal. How about if each control_... method decides on the time constant it wants to use and then updates the slew rate itself?
Then we don't need the global state anymore and just pass down the final time consatnt to tecs_update_pitch_throttle.

@Jaeyoung-Lim I agree that for fixed wing landings, proper glide slope tracking is the best solution. However, here we are using a lower TECS time constant for VTOL when they are close to the ground, e..g during a VTOL landing which has nothing to do with glide slope tracking. Having a single TECS time constant makes it difficult to have tighter altitude tracking when close to terrain while also allowing a more relaxed altitude tracking at higher altitudes, which is generally desired to fly more efficiently and to calm down the throttle.

[oravla5]

@oravla5 @Jaeyoung-Lim Thanks for your feedback.

I slightly modified the implementation removing the _is_low_height property from the FixedwingPositionControl class and made it an input parameter to the tecs_update_pitch_throttle method. Each control mode method evaluates now the low-height requirements or enforce them (in the case of landing modes) and passes the result to the tecs_update_ptich_throttle method, which then updates the TECS altitude time constant accordingly.

[Jaeyoung-Lim]

@Jaeyoung-Lim I agree that for fixed wing landings, proper glide slope tracking is the best solution. However, here we are using a lower TECS time constant for VTOL when they are close to the ground, e..g during a VTOL landing which has nothing to do with glide slope tracking. Having a single TECS time constant makes it difficult to have tighter altitude tracking when close to terrain while also allowing a more relaxed altitude tracking at higher altitudes, which is generally desired to fly more efficiently and to calm down the throttle.

@RomanBapst That makes more sense.

@oravla5 I think I am with @RomanBapst that we should have the tecs parameters as an input of control_auto_* methods, rather than the low_altitude boolean, since this is very specific to this use case. Rather if you specify the tecs time constant as a parameter, then you would be just configuring tecs behavior, rather than injecting mission logic.

• I still find it hard to understand why you would want the condition based on local altitude. There are a lot of use cases and deployments (especially in Switzerland) where the mission includes portions of the flight that fly lower than the launch altitude (or HOME). Given what @RomanBapst said, I would rather link the change of TECS time constant to the behavior (e.g. fw pos ctrl state) rather than relative altitude.

[bresch]

Please, comments are not mandatory, prefer code that reads well than a comment explaining each line of code.

[sfuhrer]

Looks okay to me, only have two minor comments.

[sfuhrer]

We're already again out of flash on make_all_variants v5: region FLASH_AXIM' overflowed by 281 bytes`

[RomanBapst]

@bresch @Jaeyoung-Lim Are you guys ok with the PR? If yes, please approve so that we can get it in.

[Jaeyoung-Lim]

I agree that having different TECS time constants are useful depending on how close the terrain is.

However, I am still concerned with how this is implemented as an independent case of TECS.

I believe we have been trying hard to make the logic flow of the whole FW pos controller tractable, since nobody actually understands how many states and transitions between the states exist in the fw_pos_control module. This PR just adds yet another case that cannot be unit tested, nor is kept track in the state machine.

• Is there a reason why the condition is_low_height as a boolean is passed to the tecs rather than the actual time constant? Some states seem to have is_low_height hard coded in the states which seems unnecessary.
• Could this be implemented as a separate FW_POSCTRL_MODE?

[RomanBapst]

I agree that having different TECS time constants are useful depending on how close the terrain is.

However, I am still concerned with how this is implemented as an independent case of TECS.

I believe we have been trying hard to make the logic flow of the whole FW pos controller tractable, since nobody actually understands how many states and transitions between the states exist in the fw_pos_control module. This PR just adds yet another case that cannot be unit tested, nor is kept track in the state machine.

* Is there a reason why the condition `is_low_height` as a boolean is passed to the tecs rather than the actual time constant? Some states seem to have `is_low_height` hard coded in the states which seems unnecessary.

* Could this be implemented as a separate `FW_POSCTRL_MODE`?

I only partially agree. I agree that it would be a bit nicer to pass the tecs time constant instead of the flag. To me it's an implementation detail that can be changed easily.
But I don't agree with the implementation being hard to trace. Initially, the implementation did actually keep a global state of the time constant and I found that hard to track. Now the implementation forces each control mode to pass the desired value for the time constant which I think makes it easy to understand. You don't need to worry about a global state that you don't know where it's updated, but you can just check in the respective control mode what the desired behavior should be. I think that is all aligned with the idea of flight tasks or flight mode manager.

[Jaeyoung-Lim]

@RomanBapst @oravla5 I am mainly concerned about implementation details here. I think I am convinced that the use case is valid. What would you think about defining the conditions as a FW_STATE, and include the condition in the state machine, such that it is clear that there are different behaviors depending on the low terrain height?

[RomanBapst]

I am mainly concerned about implementation details here. I think I am convinced that the use case is valid. What would you think about defining the conditions as a FW_STATE, and include the condition in the state machine, such that it is clear that there are different behaviors depending on the low terrain height?

@Jaeyoung-Lim I assume you mean adding a dedicated control mode for it (FW_POSCTRL_MODE)?
That would mean we need to handle all the existing control modes that should have this behavior in one new control mode just because we want to pass a different time constant to TECS?
Maybe you can explain a bit more what you were thinking to make sure we are talking about the same thing.

[Jaeyoung-Lim]

@RomanBapst Yes, I am talking about extending the enums in this:

PX4-Autopilot/src/modules/fw_pos_control/FixedwingPositionControl.hpp

Lines 251 to 263 in 8f6ce4e

	enum FW_POSCTRL_MODE {
	FW_POSCTRL_MODE_AUTO,
	FW_POSCTRL_MODE_AUTO_ALTITUDE,
	FW_POSCTRL_MODE_AUTO_CLIMBRATE,
	FW_POSCTRL_MODE_AUTO_TAKEOFF,
	FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
	FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
	FW_POSCTRL_MODE_AUTO_PATH,
	FW_POSCTRL_MODE_MANUAL_POSITION,
	FW_POSCTRL_MODE_MANUAL_ALTITUDE,
	FW_POSCTRL_MODE_TRANSITON,
	FW_POSCTRL_MODE_OTHER
	} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed

For me the different TECS constant close to the terrain is a different behavior, and be worth the specify it as a different state. For example, according to the implementation, the low height condition is not active for all states. Otherwise I struggle to understand when this condition is active and when it is not.

For example, I had to go through the code to understand whenthe low altitude logic is applied. The condition is NOT always based on terrain height.

• During a FW_POSCTRL_MODE=AUTO, the is_low_height condition is true no matter the terrain height when
  • The current waypoint is a loiter and the next waypoint is a land waypoint.
  • If the current waypoint is a loiter but it is in a landing abort state.
• During FW_POSCTRL_MODE=FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR or FW_POSCTRL_MODE=FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT, the low height conditions seem to be always true.

I am not sure if there are more cases of this, but I had to go through the code a few times to find these conditions. Therefore I think it would be easier to understand in what cases the low altitude constraints are activated and not.

We have a state machine in the module which is probably incomplete. If it is always based on terrain altitude, I agree we probably don't need to keep it as a state. However, there seems to be more logic to it, and I am just not comfortable on adding more complexity to the fw_pos_control logic. I doubt that anybody actually tried to draw out the flow diagram of different fw states.

Side note: I am not sure if this behavior is valuable for manual modes, where the stick response will change based on terrain altitude, without warning the user, but maybe you have a better feeling for this.

[sfuhrer]

For me the different TECS constant close to the terrain is a different behavior, and be worth the specify it as a different state. For example, according to the implementation, the low height condition is not active for all states. Otherwise I struggle to understand when this condition is active and when it is not.

I'm also not convinced that the TECS altitude TC variation has to be linked to a new mode. It's not black and white what a "behavioral change" is - but adjusting the weight of the controller tuning to shifting it more to tighter altitude tracking is a level below the differences the current modes have in between each other (auto vs manual, different states controlled, hard throttle and pitch limits etc).

Side note: I am not sure if this behavior is valuable for manual modes, where the stick response will change based on terrain altitude, without warning the user, but maybe you have a better feeling for this.

I don't think the stick response changes so much, as when you deflect the stick you give a direct height rate setpoint and thus the altitude TC anyway has no effect no? And for simplicity and predictability I would make all modes the same (modes and the low altitude TC tightening feature should be considered completely orthogonal).

To make it cleanly decoupled form the modes: should we consider removing the hard coded is_low_height=true for the landing/early landing config phases and really only trigger it by distance to ground?

[RomanBapst]

@bresch Do you approve?

[bresch]

For example, I had to go through the code to understand whenthe low altitude logic is applied. The condition is NOT always based on terrain height.

• During a FW_POSCTRL_MODE=AUTO, the is_low_height condition is true no matter the terrain height when
  • The current waypoint is a loiter and the next waypoint is a land waypoint.
  • If the current waypoint is a loiter but it is in a landing abort state.
• During FW_POSCTRL_MODE=FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR or FW_POSCTRL_MODE=FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT, the low height conditions seem to be always true.

To me that already sounds like a bug. I'm 100% in favor of what Silvan said:

To make it cleanly decoupled form the modes: should we consider removing the hard coded is_low_height=true for the landing/early landing config phases and really only trigger it by distance to ground?

In general, if the feature is "Support tighter altitude tracking during low-height flight" it should be kept as simple as possible and avoid the "but don't do it if the current waypoint is a loiter and the next waypoint is a land waypoint or that the current waypoint is a loiter but it is in a landing abort state".

there seems to be more logic to it, and I am just not comfortable on adding more complexity to the fw_pos_control logic. I doubt that anybody actually tried to draw out the flow diagram of different fw states.

I agree with that. I think that we should "modernize" the code a bit. My fear is that as it gets really hard to have all the cases in mind and that adding anything has then a high chance of breaking something else.

[RomanBapst]

@bresch

To me that already sounds like a bug. I'm 100% in favor of what Silvan said.

Yes, the thing is that the conditions which @Jaeyoung-Lim described are the exact same conditions that are in the code now. If we decided that we don't need them anymore because we can base everything on terrain height always, then we could remove them. However, if the distance sensor has a short range then you would get a different behavior in the sense that the TECS time constant would only be reduced at a very late stage of the landing. With the current logic, the time constant is reduced based on the waypoint type.

So in summary, the above described code is not really part of this PR and we can remove it if people are ok with it.

h

[Jaeyoung-Lim]

@RomanBapst I thought the plan was to remove the conditions with waypoint dependency and base the condition only based on terrain height?

[RomanBapst]

@Jaeyoung-Lim As mentioned previously, the waypoint dependency in the fw land mode has been there before, and all we did was preserve that logic. If the people who care about fw landing are comfortable with removing it, then I have no problem to remove it. What would it mean in practice? If you are using a lidar that has a range of 50m, then during a land approach the TECS time constant will only be set to a tighter value once you have reached a proximity of 50m to the ground. With the current logic, the TECS time constant will be set tighter based starting the approach and not just on the proximity to terrain. So even if your land approach started at 300m above ground, you would have tighter altitude tracking from the beginning, because they waypoint type tells you that you are doing an approach.