FW Pos Control: add in_takeoff_situation argument to adapt_airspeed_setpoint()

[sfuhrer]

Solved Problem

Airspeed setpoint at beginning of FW takeoff is set to AIRSPD_TRIM, and then slowly ramped down to TKO_AIRSPD.

Fixes #21567 (beside other PRs)

Solution

Add argument in_takeoff_situation to adapt_airspeed_setpoint(), and do not run airspeed setpoint slew rate limiting then. Also disable some other logic, and only keep the load factor to airspeed setpoint logic while in takeoff.

Test coverage

SITL tested.

Prior this PR:

After this PR:

Now the airspeed setpoint is at the set 12m/s when the takeoff starts, and not at the TRIM airspeed (15).

[tstastny]

A few things:

1. I'm not the biggest fan of passing a bool to the method. (this method probably anyway is doing a few too many things at once)
2. Why do we want to disable airspeed setpoint increases during takeoff? (forward ground speed check and wind variance check would actually be helpful, no? -- I've used these before on hand launches)
3. If the slew rate initializing at trim is the issue (initializes to trim because it is actually set to zero to begin with.. and the adaptAirspeed method detects this as out of range) - can we just set the initial slewed airspeed setpoint to the takeoff speed on the first pass into the resp. takeoff methods? Or if we really dont want slew rates at all, we could consider adding a bitmask input to decide what airspeed adaptations one actually wants on input to the method. But again, this somewhat already implies we should be splitting up this method.

What do you think?

[sfuhrer]

1. I'm not the biggest fan of passing a bool to the method. (this method probably anyway is doing a few too many things at once)

The function does too many things, but if it wouldn't then we would have code duplication, as the airspeed setpoint in the current module structure needs to be adapted for all the flight mode functions in it. I don't have a better idea right now, but once we split up the module into separated flight modes a clean up should definitely be possible.

2. Why do we want to disable airspeed setpoint increases during takeoff? (forward ground speed check and wind variance check would actually be helpful, no? -- I've used these before on hand launches)

The forward groundspeed check is meant to prevent flying backwards in high wind. On a takeoff we know that the groundspeed is 0, so it would trigger every time and overwrite the set takeoff airspeed, leading to a not 100% predictable outcome. Same for the wind adaption: it would only work if the vehicle was flown before and thus has a valid wind estimate, and the estimate could thus be quite wrong.
Another thing to consider is that I prefer having a flat airspeed setpoint during the takeoff phase, and not having it changing dynamically based on wind or groundspeed.

3. If the slew rate initializing at trim is the issue (initializes to trim because it is actually set to zero to begin with.. and the adaptAirspeed method detects this as out of range) - can we just set the initial slewed airspeed setpoint to the takeoff speed on the first pass into the resp. takeoff methods? Or if we really dont want slew rates at all, we could consider adding a bitmask input to decide what airspeed adaptations one actually wants on input to the method. But again, this somewhat already implies we should be splitting up this method.

Initializing it with the takeoff speed was actually my first iteration of this PR. I then though dropped it in favor of what I have now due to the reasons explained in 2.) For fixing the core issue here it would be enough, agree.

[tstastny]

Another thing to consider is that I prefer having a flat airspeed setpoint during the takeoff phase, and not having it changing dynamically based on wind or groundspeed.

I guess this depends on what behavior someone desires. Reducing speed would for sure be a nono. But increasing speed setpoint doesnt seem like something that hurts IMO (again, I used to have this enabled for handlaunch because I wanted it not to fly backwards above my head in strong wind). But I dont have an extremely strong opinion on this. If we want to disable forward ground speed maintenance for this leg, we'd need to disable both the windless correction (the one you did) and additionally the parameter(s) controlling NPFG airpseed correction behaviors (for once the wind estimate is valid, which should happen at some point shortly after takeoff):

• NPFG_EN_MIN_GSP increases airspeed to maintain a minimum forward ground speed
• NPFG_TRACK_KEEP increases airspeed to make progress towards the track
• NPFG_WIND_REG increases airspeed to combat wind (only up to the current estimated wind value.. so zero ground speed)

Of course would need to reset these then to their previous values as well directly after not to lose the generally configured behaviors.

Initializing it with the takeoff speed was actually my first iteration of this PR. I then though dropped it in favor of what I have now due to the reasons explained in 2.) For fixing the core issue here it would be enough, agree.

Well - even before the full refactor, I think we could still make this a little cleaner. E.g.

1. initialize the slewer at trim airspeed in the constructor
2. for the special case of takeoff (control_auto_takeoff() and control_manual_position/altitude()) force set the slewed value to the takeoff airspeed on first pass to the function (we have resets for takeoff modes, so these existing states can be used to deteremine if we're newly entering the mode)
3. (see clean up of the slew operations below)
4. disable the airspeed setpoint increasing operations if desired (i still dont like the bool passing, but we can live with it for now)

I re-looked at this block.. and seems a bit sketchy to me:

calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed,
				     _param_fw_airspd_max.get());

// initialize to current airspeed setpoint, also if previous setpoint is out of bounds to not apply slew rate in that case
const bool slewed_airspeed_outside_of_limits = _airspeed_slew_rate_controller.getState() < calibrated_min_airspeed
	|| _airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get();

if (in_takeoff_situation || !PX4_ISFINITE(_airspeed_slew_rate_controller.getState())
	  || slewed_airspeed_outside_of_limits) {
        _airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint);

} else if (control_interval > FLT_EPSILON) {
        // constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s
        calibrated_airspeed_setpoint = _airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval);
}
return calibrated_airspeed_setpoint;

Why are we resetting the speed to current setpoint when the slew exceeds the limits? Why not just clipping it? (is this another latent bug waiting to happen?? the current setpoint could be a big jump from the last if eg switching modes)

What about this? (also considering the bullets above.. i.e. that the takeoff speed is force set on first pass to takeoff mode .. so this method doesnt need to know anything about takeoff situation)

if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) {
        _airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint);
}
else if (control_interval > FLT_EPSILON) {
        // constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s
        calibrated_airspeed_setpoint = _airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval);
}

return constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed, _param_fw_airspd_max.get());;

The function does too many things, but if it wouldn't then we would have code duplication

Some duplication is ok if some modes want to select what airspeed modifications they want. We don't need to expose the entire contents of the method - could also just be a set of things, e.g.

• adaptAirspeedSetpointForGroundSpeed()
• adaptAirspeedSetpointForWindVariance()
• adaptAirspeedSetpointForGrossWeight()
• etc

Then if we have a specific order these need to be applied, we could still keep them inside a single method, but have a bitmask param that selects what combination one actually wants.

But we don't need to do this now. Can wait.

One more thing I thought of .. why do we need this slew before it reaches TECS, if we already have a filter there? (just something to re-look at later)

[tstastny]

also there are a ton of checks failing :S

[sfuhrer]

If we want to disable forward ground speed maintenance for this leg, we'd need to disable both the windless correction (the one you did) and additionally the parameter(s) controlling NPFG airpseed correction behaviors (for once the wind estimate is valid, which should happen at some point shortly after takeoff):

The NPFG logic only gets active once you have a valid wind estimate, while the min-groundspeed one immediately when you arm and are stationary. That's why I left out the NPFG. What I just though didn't make sense is if it increases the airspeed setpoint on the runway because it thinks it's very windy (which obviously is the wrong conclusion).

Why are we resetting the speed to current setpoint when the slew exceeds the limits? Why not just clipping it? (is this another latent bug waiting to happen??

The idea behind it is that you only should do slew rate limiting while being in the safe zone to fly. If you have a sudden increase in stall speed and thus an increased FW_AIRSPD_MIN, then you don't want to have the setpoint slew-rated, but applied immediately at a higher value.

One more thing I thought of .. why do we need this slew before it reaches TECS, if we already have a filter there? (just something to re-look at later)

Do we also filter the airspeed setpoint there? I was not aware of that.

Some duplication is ok if some modes want to select what airspeed modifications they want. We don't need to expose the entire contents of the method - could also just be a set of things, e.g.

    adaptAirspeedSetpointForGroundSpeed()
    adaptAirspeedSetpointForWindVariance()
    adaptAirspeedSetpointForGrossWeight()

etc

Good idea, though likely something I would do after the release.

[sfuhrer]

Rebased and force pushed to hopefully fix the CI failures.

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-maintainers-call-september-12-2023/34150/1

[tstastny]

ok i updated this to be in my opinion a little more aligned with our general intentions.

• airspeed constraints always hard set any violating previous state from airspeed slew controller
• airspeed setpoints are independently generated.. and either constrained or slewed within constraints
• takeoff airspeed setpoint is initialized with launch or runway init.. to make sure it doesnt need to slew down from trim

but for sure this whole logic needs some work in 1.15. stuff is too entangled.

@RomanBapst could you maybe take a look since @sfuhrer is out on holiday?

[sfuhrer]

@tstastny thanks for getting this over the line!