WIP: Fixed wing control refactoring #24056
Conversation
RomanBapst
force-pushed
the
pr-fw_ctrl_api
branch
from
a3b4779 to
d73b3a0
Compare
🔎 FLASH Analysispx4_fmu-v5x [Total VM Diff: 6972 byte (0.33 %)]px4_fmu-v6x [Total VM Diff: 6972 byte (0.34 %)]Updated: 2025-02-24T15:46:27 |
RomanBapst
force-pushed
the
pr-fw_ctrl_api
branch
3 times, most recently
from
4544dec to
daa6b1a
Compare
There was a problem hiding this comment.
I like the restructuring, having the fixed wing position control more separated into the mode and the actual control. As well as having more intermediate options.
I went through all of it. but not in the complete detail yet, just when i saw something i mentioned it.
Biggest issue for me is that we don't yet have a clear way to tell, that the longitudinal and lateral controller should be enabled or not. Should be in the vehicle control mode flags propably like _vcontrol_mode.flag_control_rates_enabled
| float32 height_rate_setpoint # NAN if not controlled directly, used as feeforward if altitude_setpoint is finite [m/s] | ||
| float32 altitude_setpoint # NAN if not controlled, MSL [m] | ||
| float32 equivalent_airspeed_setpoint # [m/s] | ||
| float32 pitch_sp # NAN if not controlled, overrides total energy controller [rad] |
There was a problem hiding this comment.
I guess you can't use those as feedforwards, right? So shouldn't we keep that separate as an alternative to the attitudeSetpoint message?
There was a problem hiding this comment.
@KonradRudin The reason that they are there is because the fixed wing takeoff and landing modes do make use of setting pitch / roll setpoints directly. I thought about having those modes publish attitude setpoints directly, however, the issue is that the FwLatLonControl module does not know about this and keeps on publishing attitude setpoints as well. The correct approach would be to set the control flags for attitude and rates only, but that requires those modes to be separated in commander. I don't yet have a good solution for this.
| float tecs_fw_thr_max; | ||
| const float acc_rad = _directional_guidance.switchDistance(500.0f); | ||
| float throttle_min = _param_fw_thr_min.get(); | ||
| float throttle_max = _param_fw_thr_max.get(); | ||
|
|
||
| if (pos_sp_curr.gliding_enabled) { |
There was a problem hiding this comment.
I think this is currently not handled, right? Check comment about the uorb topic, would be nice to have in the long control a field where you can specify normal mode or like here thrust/speed mode
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
Outdated
Show resolved
Hide resolved
There was a problem hiding this comment.
Sorry that I am late to comment on this PR. I think there would be a lot of value of creating a FW API.
The api proposed have separate messages for longitudinal and lateral gudiance. This seems to be built on the assumption that lateral and longitudinal control (TECS) would be controlled in a decoupled way.
However, it would have been nicer if the api would be designed for more "generic" fixed-wing tasks, rather than exposing configurations of the underlying interface. IMO, an api should fully define what the vehicle "should" do for a given task (e.g. path following), rather than expose the underlying implementation.
-
Lateral/longitudinal controller decoupling is a design choice for a specific implementation. This would make it hard for other controllers to share the API, if the controllers do not have such decoupling (e.g. 3D NDI acceleration-based control).
-
The decoupling is not only not necessary for the task of path following, but can even limit what kind of paths we can follow in 3D. For example, if you try to track an inclined loiter(or any complex 3D path of arbitrary shape) the height rate setpoint in the
FwLongitudinalSetpoint.msgis no longer usable, since the information on the 3D geometry is already lost (e.g. height rate depends on the speed and direction of the vehicle to stay on the path).
Is there a good reason that we want to enforce the separation of lateral/longitudinal control on the API side?
@tstastny
|
@Jaeyoung-Lim Thanks for your input on this!
Yes, and so far lateral and longitudinal control have almost completely been decoupled in PX4. The only coupling I am aware of is that NPFG used to be able to change the airspeed setpoint. But other than that, it was decoupled.
You are not limited to the exposed interfaces presented in this PR. If you want to have your fancy NDI controller running offboard and send attitude / rate or even actuator commands directly, you are free to do so, those interfaces I think are already there.
I think it's a design choice which solves 95% of the use case that people would want. I think that's pretty good.
Could you please specify the exact interface that you would need to solve whatever use case you are trying to solve? Generally, I have a feeling that your expectation is that ONE uorb topic defines exactly ONE control API. I don't think this needs to be true necessarily. I think you will use some kind of SDK, which will expose one interface to the developer, but the SDK itself can use multiple uorb topics. |
|
Hi. Thanks @RomanBapst and @KonradRudin for putting in work to finally start cleaning up the the fixed-wing high-level control infrastructure a bit! I haven't had time to fully go through the details, but I am a little confused what specifically is the "API" here. But there may just be some difference in everyone's semantics... The uorb topics in the figure look more like logging topics to me, as I find it odd an application would send e.g. pitch setpoint, throttle setpoint, altitude setpoint, and height rate setpoint all out into the ether at the same time? Maybe @RomanBapst you can de-confuse me on the strategy? :) Are there multiple uorbs being bundled and sent at once from e.g. the ROS2 mode? Are these messages just defining what each block (controller) needs as input? And the actual exposed API will be something more basic, with each control block parsing it to convert to the right inputs to each? Maybe it would help to include the module boundaries on the diagram, so it's clear where uorbs are subscribed to, vs whatever internal parsing a module does to input the right args to the control libraries. On the topic of alternate APIs (@Jaeyoung-Lim ), the block diagram itself looks as though it's not terribly far away from supporting a wide array of things. e.g. if there were a single 3d instantaneous path setpoint API, the height component of it could still be routed to Alt-Ctrl, the vertical incline of the path then converted to a height rate setpoint that could be used as feedforward, added to the feedback height rate from Alt-Ctrl, and then sent off to TECS (or Velocity NDI, or whatever). It looks as though that is actually already somewhat happening in the diagram? So long as the blocks on the diagram are decoupled as shown, and new urobs can be made with a parser to spit out the resp. input msgs each controller one is using needs, I would imagine it's possible to do, right? Or maybe I miss something on the routing details? (I am a bit foggy on things now after not looking for a long time) |
RomanBapst
changed the title
@tstastny I have changed the title of the PR to not contain the word API, as it seems to be confusing. Also note, that the current uorb messages contain certain fields I wish would not be there, but since it was a priority to not break existing functionality with this refactor, I added them regardless. I am all in favor to remove fields like I'm also adding a diagram here which I hope explains a bit better the architecture in terms of modularity. |
RomanBapst
force-pushed
the
pr-fw_ctrl_api
branch
from
60aa9d9 to
55be964
Compare
| float32 course_setpoint # [-pi, pi], NAN if not controlled directly | ||
| float32 airspeed_reference_direction # [-pi, pi], angle of desired airspeed vector, NAN if not controlled directly, used as feedforward if course setpoint is finite |
There was a problem hiding this comment.
Looks like the other elements in the messages are described with units, [rad] could be added here as well.
There was a problem hiding this comment.
and we need to add if it's a true airspeed or calibrated/indicated/equivalent
| float32 course_setpoint # [-pi, pi], NAN if not controlled directly | ||
| float32 airspeed_reference_direction # [-pi, pi], angle of desired airspeed vector, NAN if not controlled directly, used as feedforward if course setpoint is finite | ||
| float32 lateral_acceleration_setpoint # [m/s^2], NAN if not controlled directly, used as feedforward if either course setpoint or airspeed_reference_direction is finite | ||
| float32 roll_sp # TODO: remove, only for testing |
…responding limits Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
Signed-off-by: RomanBapst <[email protected]>
…acecraft Signed-off-by: Silvan <[email protected]>
…ngtip strike Signed-off-by: RomanBapst <[email protected]>
* bit of cosmetic cleanup Signed-off-by: Silvan <[email protected]> * AttitudeSetpoint.msg: remove reset_integral The functionality of resetting the rate controller integrals when the position controller deems it necessary adds a lot of complexity for low benefits and is thus removed here. Signed-off-by: Silvan <[email protected]> * FwLateralControlSetpoint.msg: remove reset_integral Signed-off-by: Silvan <[email protected]> * VehicleAttitudeSetpoint: remove fw_control_yaw_wheel This info can be passed to the wheel controller via a separate message (e.g. runway_takeoff_status). What about landing anyway? We don't have any auto wheel control there yet. Signed-off-by: Silvan <[email protected]> * remove passing of yaw setpoint from FW Pos Control to attitude (for wheel) Signed-off-by: Silvan <[email protected]> * remove NPFG guidance when on ground Signed-off-by: Silvan <[email protected]> --------- Signed-off-by: Silvan <[email protected]> Co-authored-by: Roman Bapst <[email protected]>
- only publish if they changed Signed-off-by: RomanBapst <[email protected]>
RomanBapst
force-pushed
the
pr-fw_ctrl_api
branch
from
5dff39d to
6a9a8c4
Compare
| return can_run_factor * (lateral_accel_sp); | ||
| } | ||
| float FwLateralLongitudinalControl::mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const { | ||
| return atanf(lateral_acceleration_sp * 1.0f / CONSTANTS_ONE_G); |
There was a problem hiding this comment.
After discussion we can remove the 1.0f from the function.
| fw_lateral_control_setpoint_s status = { | ||
| .timestamp = _lat_control_sp.timestamp, | ||
| .course_setpoint = _lat_control_sp.course_setpoint, | ||
| .airspeed_reference_direction = airspeed_reference_direction, | ||
| .lateral_acceleration_setpoint = lateral_accel_sp, | ||
| .roll_sp = roll_sp // TODO: just for logging, can be removed later | ||
| }; |
There was a problem hiding this comment.
Add a comment that its only for logging and not consumed by any other
| uORB::PublicationData <fw_lateral_control_setpoint_s> _lateral_ctrl_status_pub{ORB_ID(fw_lateral_control_status)}; | ||
| uORB::PublicationData <fw_longitudinal_control_setpoint_s> _longitudinal_ctrl_status_pub{ |
There was a problem hiding this comment.
should it be type uORB::Publication ?
| @@ -0,0 +1,63 @@ | |||
| /**************************************************************************** | |||
| * | |||
| * Copyright (c) 2024 PX4 Development Team. All rights reserved. | |||
There was a problem hiding this comment.
| * Copyright (c) 2024 PX4 Development Team. All rights reserved. | |
| * Copyright (c) 2025 PX4 Development Team. All rights reserved. |
saw it in a few other places as well
| * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] | ||
| * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] | ||
| * @param[in] wind_speed Wind speed [m/s] | ||
| * @param[in] min_ground_speed Minimum commanded forward ground speed [m/s] |
There was a problem hiding this comment.
min_ground_speed is no longer an input
| // must be called before trackErrorBound() as it updates time_const_ | ||
| adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error, | ||
| path_curvature, wind_vel, unit_path_tangent, feas_on_track_); | ||
| _time_const = timeConst(adapted_period_, damping_); |
There was a problem hiding this comment.
| _time_const = timeConst(adapted_period_, damping_); | |
| const float time_const = timeConst(adapted_period_, damping_); |
As it's no longer used outside the scope of this function and then remove the class variable in the headerfile
| // must be called before trackErrorBound() as it updates time_const_ | ||
| adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error, | ||
| path_curvature, wind_vel, unit_path_tangent, feas_on_track_); | ||
| p_gain_ = pGain(adapted_period_, damping_); |
There was a problem hiding this comment.
Is an adaptive gain not needed anymore?
| float throttle_sp{NAN}; | ||
|
|
||
| if (_fw_longitudinal_ctrl_sub.updated()) { | ||
| _fw_longitudinal_ctrl_sub.copy(&_long_control_sp); |
There was a problem hiding this comment.
| _fw_longitudinal_ctrl_sub.copy(&_long_control_sp); | |
| fw_longitudinal_control_setpoint_s long_control_sp | |
| _fw_longitudinal_ctrl_sub.copy(&long_control_sp); |
Looks like it's only in this scope so definition can be within the scope instead of the headerfile
|
|
||
| if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) { | ||
| _need_report_npfg_uncertain_condition = false; | ||
| events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning, |
There was a problem hiding this comment.
| events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning, | |
| events::send(events::ID("fw_lateral_longitudinal_control_roll_command_uncertain"), events::Log::Warning, |
as it's now in the module and not in the lib
| _long_limits.timestamp = hrt_absolute_time(); | ||
| _long_limits.equivalent_airspeed_min = _performance_model.getMinimumCalibratedAirspeed(); | ||
| _long_limits.equivalent_airspeed_max = _performance_model.getMaximumCalibratedAirspeed(); | ||
| _long_limits.pitch_min = radians(_param_fw_p_lim_min.get()); | ||
| _long_limits.pitch_max = radians(_param_fw_p_lim_max.get()); | ||
| _long_limits.throttle_min = _param_fw_thr_min.get(); | ||
| _long_limits.throttle_max = _param_fw_thr_max.get(); | ||
| _long_limits.climb_rate_target = _param_climbrate_target.get(); | ||
| _long_limits.sink_rate_target = _param_sinkrate_target.get(); | ||
| _long_limits.disable_underspeed_protection = false; | ||
| _long_limits.enforce_low_height_condition = false; |
| _longitudinal_ctrl_status_pub.publish(longitudinal_control_status); | ||
|
|
||
| float roll_sp {NAN}; | ||
| float yaw_sp {NAN}; |
There was a problem hiding this comment.
Shall the yaw_sp be removed?
| _long_limits.disable_underspeed_protection, | ||
| _long_control_sp.height_rate_setpoint | ||
| ); | ||
| pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_sp) ? _long_control_sp.pitch_sp : _tecs.get_pitch_setpoint(); |
There was a problem hiding this comment.
The pitch setpoint used to have a protection in case the tecs is not running:
`
if (_tecs_is_running) {
return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get());
}
// return level flight pitch offset to prevent stale tecs state when it's not running
return radians(_param_fw_psp_off.get());
`
Shall we add a method for that here?
| _long_control_sp.height_rate_setpoint | ||
| ); | ||
| pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_sp) ? _long_control_sp.pitch_sp : _tecs.get_pitch_setpoint(); | ||
| throttle_sp = PX4_ISFINITE(_long_control_sp.thrust_sp) ? _long_control_sp.thrust_sp : _tecs.get_throttle_setpoint(); |
There was a problem hiding this comment.
The throttle also had some protection:
`
if (_tecs_is_running) {
return min(_tecs.get_throttle_setpoint(), 1.f);
// return 0 to prevent stale tecs state when it's not running
return 0.0f;`
Solved Problem
Implement fixed wing control API for lateral and longitudinal control.
Changelog Entry
For release notes:
TODO
Test in simulation all modes
Stabilized
Altitude
Position
Hold
Mission
RTL
Orbit
Finalize handling of control limits
Verify that fixed wing auto-takeoff and auto-landing still work as intended
Solve Flash
Alternatives
We could also ...
Test coverage
Context
Related links, screenshot before/after, video