dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FW Position Controller: change airspeed setpoint init 

-remove dedicated vtol transition airspeed init logic
-init airspeed setpoint on first usage of tecs
-init to max of current airspeed and min airspeed

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-12-06 10:59:39 +01:00
parent 27957e1f2f
commit 5211c358aa
2 changed files with 15 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -59,9 +59,6 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
, _figure_eight(_npfg, _wind_vel, _eas2tas)
#endif // CONFIG_FIGURE_OF_EIGHT
{
if (vtol) {
_param_handle_airspeed_trans = param_find("VT_ARSP_TRANS");
}
// limit to 50 Hz
_local_pos_sub.set_interval_ms(20);
@ -80,7 +77,6 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
/* fetch initial parameter values */
parameters_update();
_airspeed_slew_rate_controller.setForcedValue(_performance_model.getCalibratedTrimAirspeed());
_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
_roll_slew_rate.setForcedValue(0.f);
@ -111,11 +107,6 @@ FixedwingPositionControl::parameters_update()
_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
// VTOL parameter VT_ARSP_TRANS
if (_param_handle_airspeed_trans != PARAM_INVALID) {
param_get(_param_handle_airspeed_trans, &_param_airspeed_trans);
}
// NPFG parameters
_npfg.setPeriod(_param_npfg_period.get());
_npfg.setDamping(_param_npfg_damping.get());
@ -394,8 +385,15 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed,
_performance_model.getMaximumCalibratedAirspeed());
if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) {
_airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint);
// initialize the airspeed setpoint to the max(current airsped, min airspeed)
if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())
|| !_tecs_is_running) {
_airspeed_slew_rate_controller.setForcedValue(math::max(calibrated_min_airspeed, _airspeed_eas));
}
// reset the airspeed setpoint to the min airspeed if the min airspeed changes while in operation
if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) {
_airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed);
}
if (control_interval > FLT_EPSILON) {
@ -403,10 +401,6 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
_airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval);
}
if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) {
_airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed);
}
if (_airspeed_slew_rate_controller.getState() > _performance_model.getMaximumCalibratedAirspeed()) {
_airspeed_slew_rate_controller.setForcedValue(_performance_model.getMaximumCalibratedAirspeed());
}
@ -2519,48 +2513,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
const float desired_max_sinkrate, const float desired_max_climbrate,
bool disable_underspeed_detection, float hgt_rate_sp)
{
_tecs_is_running = true;
// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
// (it should also not run during VTOL blending because airspeed is too low still)
if (_vehicle_status.is_vtol) {
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) {
_tecs_is_running = false;
}
if (_vehicle_status.in_transition_mode) {
// we're in transition
_was_in_transition = true;
// set this to transition airspeed to init tecs correctly
if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) {
// some vtols fly without airspeed sensor
_airspeed_after_transition = _param_airspeed_trans;
} else {
_airspeed_after_transition = _airspeed_eas;
}
_airspeed_after_transition = constrain(_airspeed_after_transition,
_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()),
_performance_model.getMaximumCalibratedAirspeed());
} else if (_was_in_transition) {
// after transition we ramp up desired airspeed from the speed we had coming out of the transition
_airspeed_after_transition += control_interval * 2.0f; // increase 2m/s
if (_airspeed_after_transition < airspeed_sp && _airspeed_eas < airspeed_sp) {
airspeed_sp = max(_airspeed_after_transition, _airspeed_eas);
} else {
_was_in_transition = false;
_airspeed_after_transition = 0.0f;
}
}
}
if (!_tecs_is_running) {
if (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|| _vehicle_status.in_transition_mode)) {
_tecs_is_running = false;
return;
} else {
_tecs_is_running = true;
}
/* update TECS vehicle state estimates */

5
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -384,13 +384,8 @@ private:
bool _tecs_is_running{false};
// VTOL / TRANSITION
float _airspeed_after_transition{0.0f};
bool _was_in_transition{false};
bool _is_vtol_tailsitter{false};
matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
param_t _param_handle_airspeed_trans{PARAM_INVALID};
float _param_airspeed_trans{NAN}; // [m/s]
// ESTIMATOR RESET COUNTERS