|
|
|
@@ -0,0 +1,680 @@
|
|
|
|
|
/****************************************************************************
|
|
|
|
|
*
|
|
|
|
|
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
|
|
|
|
*
|
|
|
|
|
* Redistribution and use in source and binary forms, with or without
|
|
|
|
|
* modification, are permitted provided that the following conditions
|
|
|
|
|
* are met:
|
|
|
|
|
*
|
|
|
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
|
|
|
* notice, this list of conditions and the following disclaimer.
|
|
|
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
|
|
|
* notice, this list of conditions and the following disclaimer in
|
|
|
|
|
* the documentation and/or other materials provided with the
|
|
|
|
|
* distribution.
|
|
|
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
|
|
|
* used to endorse or promote products derived from this software
|
|
|
|
|
* without specific prior written permission.
|
|
|
|
|
*
|
|
|
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
|
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
|
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
|
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
|
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
|
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
|
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
|
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
|
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
|
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
|
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
|
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
|
*
|
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
|
|
#include "FwLateralLongitudinalControl.hpp"
|
|
|
|
|
#include <px4_platform_common/events.h>
|
|
|
|
|
|
|
|
|
|
using math::constrain;
|
|
|
|
|
using math::max;
|
|
|
|
|
using math::radians;
|
|
|
|
|
|
|
|
|
|
using matrix::Dcmf;
|
|
|
|
|
using matrix::Eulerf;
|
|
|
|
|
using matrix::Quatf;
|
|
|
|
|
using matrix::Vector2f;
|
|
|
|
|
|
|
|
|
|
// [m/s] maximum reference altitude rate threshhold
|
|
|
|
|
static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f;
|
|
|
|
|
// [us] time after which the wind estimate is disabled if no longer updating
|
|
|
|
|
static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s;
|
|
|
|
|
// [s] slew rate with which we change altitude time constant
|
|
|
|
|
static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f;
|
|
|
|
|
|
|
|
|
|
static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_VALID{3.f}; ///< Factor of velocity standard deviation above which course calculation is considered good enough
|
|
|
|
|
static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_INVALID{2.f}; ///< Factor of velocity standard deviation below which course calculation is considered unsafe
|
|
|
|
|
static constexpr float COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK{0.09f}; ///< Cos of Heading to track angle below which it is assumed that the vehicle is not pushed back by the wind ~cos(85°)
|
|
|
|
|
static constexpr float COS_HEADING_TRACK_ANGLE_PUSHED_BACK{0.f}; ///< Cos of Heading to track angle above which it is assumed that the vehicle is pushed back by the wind
|
|
|
|
|
|
|
|
|
|
// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered
|
|
|
|
|
static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s;
|
|
|
|
|
|
|
|
|
|
// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning
|
|
|
|
|
static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f;
|
|
|
|
|
|
|
|
|
|
const fw_lateral_control_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course_setpoint = NAN, .airspeed_reference_direction = NAN, .lateral_acceleration_setpoint = NAN, .roll_sp = NAN};
|
|
|
|
|
const fw_longitudinal_control_setpoint_s empty_longitudinal_control_setpoint = {.timestamp = 0, .height_rate_setpoint = NAN, .altitude_setpoint = NAN, .equivalent_airspeed_setpoint = NAN, .pitch_sp = NAN, .thrust_sp = NAN};
|
|
|
|
|
|
|
|
|
|
FwLateralLongitudinalControl::FwLateralLongitudinalControl(bool is_vtol) :
|
|
|
|
|
ModuleParams(nullptr),
|
|
|
|
|
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
|
|
|
|
|
_attitude_sp_pub(is_vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
|
|
|
|
|
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
|
|
|
|
|
{
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
FwLateralLongitudinalControl::~FwLateralLongitudinalControl()
|
|
|
|
|
{
|
|
|
|
|
perf_free(_loop_perf);
|
|
|
|
|
}
|
|
|
|
|
void
|
|
|
|
|
FwLateralLongitudinalControl::parameters_update()
|
|
|
|
|
{
|
|
|
|
|
updateParams();
|
|
|
|
|
_tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density));
|
|
|
|
|
_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
|
|
|
|
|
_tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density));
|
|
|
|
|
_tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed());
|
|
|
|
|
_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed());
|
|
|
|
|
_tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed());
|
|
|
|
|
_tecs.set_throttle_damp(_param_fw_t_thr_damping.get());
|
|
|
|
|
_tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get());
|
|
|
|
|
_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
|
|
|
|
|
_tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get());
|
|
|
|
|
_tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
|
|
|
|
|
_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
|
|
|
|
|
_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
|
|
|
|
|
_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
|
|
|
|
|
_tecs.set_fast_descend_altitude_error(_param_fw_t_fast_alt_err.get());
|
|
|
|
|
_tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get());
|
|
|
|
|
_tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
|
|
|
|
|
_tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get());
|
|
|
|
|
_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
|
|
|
|
|
_tecs.set_airspeed_measurement_std_dev(_param_speed_standard_dev.get());
|
|
|
|
|
_tecs.set_airspeed_rate_measurement_std_dev(_param_speed_rate_standard_dev.get());
|
|
|
|
|
_tecs.set_airspeed_filter_process_std_dev(_param_process_noise_standard_dev.get());
|
|
|
|
|
|
|
|
|
|
_performance_model.runSanityChecks();
|
|
|
|
|
|
|
|
|
|
_performance_model.updateParameters();
|
|
|
|
|
_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
|
|
|
|
|
|
|
|
|
|
_tecs_alt_time_const_slew_rate.setSlewRate(TECS_ALT_TIME_CONST_SLEW_RATE);
|
|
|
|
|
_tecs_alt_time_const_slew_rate.setForcedValue(_param_fw_t_h_error_tc.get() * _param_fw_thrtc_sc.get());
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void FwLateralLongitudinalControl::Run()
|
|
|
|
|
{
|
|
|
|
|
if (should_exit()) {
|
|
|
|
|
_local_pos_sub.unregisterCallback();
|
|
|
|
|
exit_and_cleanup();
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
perf_begin(_loop_perf);
|
|
|
|
|
|
|
|
|
|
/* only run controller if position changed */
|
|
|
|
|
// check for parameter updates
|
|
|
|
|
if (_parameter_update_sub.updated()) {
|
|
|
|
|
// clear update
|
|
|
|
|
parameter_update_s pupdate;
|
|
|
|
|
_parameter_update_sub.copy(&pupdate);
|
|
|
|
|
|
|
|
|
|
// update parameters from storage
|
|
|
|
|
parameters_update();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (_local_pos_sub.update(&_local_pos)) {
|
|
|
|
|
|
|
|
|
|
const float control_interval = math::constrain((_local_pos.timestamp - _last_time_loop_ran) * 1e-6f,
|
|
|
|
|
0.001f, 0.1f);
|
|
|
|
|
_last_time_loop_ran = _local_pos.timestamp;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
_long_control_limits_sub.update();
|
|
|
|
|
_lateral_control_limits_sub.update();
|
|
|
|
|
|
|
|
|
|
_tecs.set_speed_weight(_long_control_limits_sub.get().speed_weight);
|
|
|
|
|
updateTECSAltitudeTimeConstant(checkLowHeightConditions()
|
|
|
|
|
|| _long_control_limits_sub.get().enforce_low_height_condition, control_interval);
|
|
|
|
|
_tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState());
|
|
|
|
|
|
|
|
|
|
if (_vehicle_air_data_sub.updated()) {
|
|
|
|
|
_air_density = PX4_ISFINITE(_vehicle_air_data_sub.get().rho) ? _vehicle_air_data_sub.get().rho : _air_density;
|
|
|
|
|
_tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density));
|
|
|
|
|
_tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (_vehicle_landed_sub.updated()) {
|
|
|
|
|
vehicle_land_detected_s landed{};
|
|
|
|
|
_vehicle_landed_sub.copy(&landed);
|
|
|
|
|
_landed = landed.landed;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
|
|
|
|
|
|
|
|
|
|
_vehicle_status_sub.update();
|
|
|
|
|
_control_mode_sub.update();
|
|
|
|
|
update_control_state();
|
|
|
|
|
|
|
|
|
|
if (_control_mode_sub.get().flag_control_manual_enabled && _control_mode_sub.get().flag_control_altitude_enabled
|
|
|
|
|
&& _local_pos.z_reset_counter != _z_reset_counter) {
|
|
|
|
|
if (_control_mode_sub.get().flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) {
|
|
|
|
|
// make TECS accept step in altitude and demanded altitude
|
|
|
|
|
_tecs.handle_alt_step(_long_control_state.altitude_msl, _long_control_state.height_rate);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float pitch_sp{NAN};
|
|
|
|
|
float throttle_sp{NAN};
|
|
|
|
|
|
|
|
|
|
bool publish = false;
|
|
|
|
|
|
|
|
|
|
fw_longitudinal_control_setpoint_s longitudinal_sp = {empty_longitudinal_control_setpoint};
|
|
|
|
|
_fw_longitudinal_ctrl_sub.copy(&longitudinal_sp);
|
|
|
|
|
|
|
|
|
|
if (longitudinal_sp.timestamp > 0 && hrt_elapsed_time(&longitudinal_sp.timestamp) < 1_s) {
|
|
|
|
|
publish = true;
|
|
|
|
|
tecs_update_pitch_throttle(control_interval, longitudinal_sp.altitude_setpoint,
|
|
|
|
|
PX4_ISFINITE(longitudinal_sp.equivalent_airspeed_setpoint)
|
|
|
|
|
? longitudinal_sp.equivalent_airspeed_setpoint :
|
|
|
|
|
_performance_model.getCalibratedTrimAirspeed(),
|
|
|
|
|
_long_control_limits_sub.get().pitch_min,
|
|
|
|
|
_long_control_limits_sub.get().pitch_max,
|
|
|
|
|
_long_control_limits_sub.get().throttle_min,
|
|
|
|
|
_long_control_limits_sub.get().throttle_max,
|
|
|
|
|
_long_control_limits_sub.get().sink_rate_max,
|
|
|
|
|
_long_control_limits_sub.get().climb_rate_max,
|
|
|
|
|
_long_control_limits_sub.get().disable_underspeed_protection,
|
|
|
|
|
longitudinal_sp.height_rate_setpoint
|
|
|
|
|
);
|
|
|
|
|
pitch_sp = PX4_ISFINITE(longitudinal_sp.pitch_sp) ? longitudinal_sp.pitch_sp : _tecs.get_pitch_setpoint();
|
|
|
|
|
throttle_sp = PX4_ISFINITE(longitudinal_sp.thrust_sp) ? longitudinal_sp.thrust_sp : _tecs.get_throttle_setpoint();
|
|
|
|
|
|
|
|
|
|
fw_longitudinal_control_setpoint_s longitudinal_control_status {
|
|
|
|
|
.timestamp = hrt_absolute_time(),
|
|
|
|
|
.height_rate_setpoint = _tecs.getStatus().control.altitude_rate_control,
|
|
|
|
|
.altitude_setpoint = longitudinal_sp.altitude_setpoint,
|
|
|
|
|
.equivalent_airspeed_setpoint = _tecs.getStatus().true_airspeed_sp / _long_control_state.eas2tas,
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
_longitudinal_ctrl_status_pub.publish(longitudinal_control_status);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float roll_sp {NAN};
|
|
|
|
|
float yaw_sp {NAN};
|
|
|
|
|
|
|
|
|
|
fw_lateral_control_setpoint_s lateral_sp = {empty_lateral_control_setpoint};
|
|
|
|
|
_fw_lateral_ctrl_sub.copy(&lateral_sp);
|
|
|
|
|
|
|
|
|
|
if (lateral_sp.timestamp > 0 && hrt_elapsed_time(&lateral_sp.timestamp) < 1_s) {
|
|
|
|
|
publish = true;
|
|
|
|
|
float airspeed_reference_direction{NAN};
|
|
|
|
|
float lateral_accel_sp {NAN};
|
|
|
|
|
|
|
|
|
|
if (PX4_ISFINITE(lateral_sp.course_setpoint)) {
|
|
|
|
|
airspeed_reference_direction = _course_to_airspeed.mapCourseSetpointToHeadingSetpoint(
|
|
|
|
|
lateral_sp.course_setpoint, _lateral_control_state.wind_speed,
|
|
|
|
|
_long_control_state.airspeed_eas * _long_control_state.eas2tas);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (PX4_ISFINITE(lateral_sp.airspeed_reference_direction)) {
|
|
|
|
|
airspeed_reference_direction = PX4_ISFINITE(airspeed_reference_direction) ? airspeed_reference_direction +
|
|
|
|
|
lateral_sp.airspeed_reference_direction : lateral_sp.airspeed_reference_direction;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (PX4_ISFINITE(airspeed_reference_direction)) {
|
|
|
|
|
const Vector2f airspeed_vector = _lateral_control_state.ground_speed - _lateral_control_state.wind_speed;
|
|
|
|
|
const float heading = atan2f(airspeed_vector(1), airspeed_vector(0));
|
|
|
|
|
lateral_accel_sp = _airspeed_ref_control.controlHeading(airspeed_reference_direction, heading,
|
|
|
|
|
_long_control_state.airspeed_eas * _long_control_state.eas2tas);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (PX4_ISFINITE(lateral_sp.lateral_acceleration_setpoint)) {
|
|
|
|
|
lateral_accel_sp = PX4_ISFINITE(lateral_accel_sp) ? lateral_accel_sp + lateral_sp.lateral_acceleration_setpoint :
|
|
|
|
|
lateral_sp.lateral_acceleration_setpoint;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (PX4_ISFINITE(lateral_accel_sp)) {
|
|
|
|
|
lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp);
|
|
|
|
|
lateral_accel_sp = math::constrain(lateral_accel_sp, -_lateral_control_limits_sub.get().lateral_accel_max,
|
|
|
|
|
_lateral_control_limits_sub.get().lateral_accel_max);
|
|
|
|
|
roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_att_sp.reset_integral = lateral_sp.reset_integral;
|
|
|
|
|
_att_sp.fw_control_yaw_wheel = PX4_ISFINITE(lateral_sp.heading_sp_runway_takeoff);
|
|
|
|
|
yaw_sp = lateral_sp.heading_sp_runway_takeoff;
|
|
|
|
|
|
|
|
|
|
fw_lateral_control_setpoint_s status = {
|
|
|
|
|
.timestamp = lateral_sp.timestamp,
|
|
|
|
|
.course_setpoint = lateral_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
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
_lateral_ctrl_status_pub.publish(status);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (publish) {
|
|
|
|
|
float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f;
|
|
|
|
|
float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f;
|
|
|
|
|
const float yaw_body = PX4_ISFINITE(yaw_sp) ? yaw_sp : _yaw;
|
|
|
|
|
const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f;
|
|
|
|
|
|
|
|
|
|
if (_control_mode_sub.get().flag_control_manual_enabled) {
|
|
|
|
|
roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()),
|
|
|
|
|
radians(_param_fw_r_lim.get()));
|
|
|
|
|
pitch_body = constrain(pitch_body, radians(_param_fw_p_lim_min.get()),
|
|
|
|
|
radians(_param_fw_p_lim_max.get()));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (_control_mode_sub.get().flag_control_position_enabled ||
|
|
|
|
|
_control_mode_sub.get().flag_control_velocity_enabled ||
|
|
|
|
|
_control_mode_sub.get().flag_control_acceleration_enabled ||
|
|
|
|
|
_control_mode_sub.get().flag_control_altitude_enabled ||
|
|
|
|
|
_control_mode_sub.get().flag_control_climb_rate_enabled) {
|
|
|
|
|
|
|
|
|
|
// roll slew rate
|
|
|
|
|
roll_body = _roll_slew_rate.update(roll_body, control_interval);
|
|
|
|
|
|
|
|
|
|
_att_sp.timestamp = hrt_absolute_time();
|
|
|
|
|
const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
|
|
|
|
|
q.copyTo(_att_sp.q_d);
|
|
|
|
|
|
|
|
|
|
_att_sp.thrust_body[0] = thrust_body_x;
|
|
|
|
|
|
|
|
|
|
_attitude_sp_pub.publish(_att_sp);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_z_reset_counter = _local_pos.z_reset_counter;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
perf_end(_loop_perf);
|
|
|
|
|
}
|
|
|
|
|
void
|
|
|
|
|
FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
|
|
|
|
|
float pitch_min_rad, float pitch_max_rad, float throttle_min,
|
|
|
|
|
float throttle_max, const float desired_max_sinkrate,
|
|
|
|
|
const float desired_max_climbrate,
|
|
|
|
|
bool disable_underspeed_detection, float hgt_rate_sp)
|
|
|
|
|
{
|
|
|
|
|
// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
|
|
|
|
|
if (_vehicle_status_sub.get().is_vtol
|
|
|
|
|
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|
|
|
|
|
|| _vehicle_status_sub.get().in_transition_mode)) {
|
|
|
|
|
_tecs_is_running = false;
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
_tecs_is_running = true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min,
|
|
|
|
|
throttle_max, airspeed_sp, _air_density);
|
|
|
|
|
|
|
|
|
|
/* No underspeed protection in landing mode */
|
|
|
|
|
_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
|
|
|
|
|
|
|
|
|
|
//updateTECSAltitudeTimeConstant(is_low_height, control_interval);
|
|
|
|
|
|
|
|
|
|
// HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases
|
|
|
|
|
// when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0.
|
|
|
|
|
const float airspeed_rate_estimate = 0.f;
|
|
|
|
|
|
|
|
|
|
_tecs.update(_long_control_state.pitch_rad - radians(_param_fw_psp_off.get()),
|
|
|
|
|
_long_control_state.altitude_msl,
|
|
|
|
|
alt_sp,
|
|
|
|
|
airspeed_sp,
|
|
|
|
|
_long_control_state.airspeed_eas,
|
|
|
|
|
_long_control_state.eas2tas,
|
|
|
|
|
throttle_min,
|
|
|
|
|
throttle_max,
|
|
|
|
|
throttle_trim_compensated,
|
|
|
|
|
pitch_min_rad - radians(_param_fw_psp_off.get()),
|
|
|
|
|
pitch_max_rad - radians(_param_fw_psp_off.get()),
|
|
|
|
|
desired_max_climbrate,
|
|
|
|
|
desired_max_sinkrate,
|
|
|
|
|
airspeed_rate_estimate,
|
|
|
|
|
_long_control_state.height_rate,
|
|
|
|
|
hgt_rate_sp);
|
|
|
|
|
|
|
|
|
|
tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated);
|
|
|
|
|
|
|
|
|
|
if (_tecs_is_running && !_vehicle_status_sub.get().in_transition_mode
|
|
|
|
|
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
|
|
|
|
|
const TECS::DebugOutput &tecs_output{_tecs.getStatus()};
|
|
|
|
|
|
|
|
|
|
// Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving
|
|
|
|
|
if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) &&
|
|
|
|
|
fabsf(_long_control_state.altitude_msl - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) {
|
|
|
|
|
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL;
|
|
|
|
|
|
|
|
|
|
} else if (((tecs_output.altitude_reference - _long_control_state.altitude_msl) >= _param_nav_fw_alt_rad.get()) ||
|
|
|
|
|
(tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
|
|
|
|
|
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB;
|
|
|
|
|
|
|
|
|
|
} else if (((_long_control_state.altitude_msl - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) ||
|
|
|
|
|
(tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
|
|
|
|
|
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
//We can't infer the flight phase , do nothing, estimation is reset at each step
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void
|
|
|
|
|
FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
|
|
|
|
|
float true_airspeed_derivative_raw, float throttle_trim)
|
|
|
|
|
{
|
|
|
|
|
tecs_status_s tecs_status{};
|
|
|
|
|
|
|
|
|
|
const TECS::DebugOutput &debug_output{_tecs.getStatus()};
|
|
|
|
|
|
|
|
|
|
tecs_status.altitude_sp = alt_sp;
|
|
|
|
|
tecs_status.altitude_reference = debug_output.altitude_reference;
|
|
|
|
|
tecs_status.altitude_time_constant = _tecs.get_altitude_error_time_constant();
|
|
|
|
|
tecs_status.height_rate_reference = debug_output.height_rate_reference;
|
|
|
|
|
tecs_status.height_rate_direct = debug_output.height_rate_direct;
|
|
|
|
|
tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control;
|
|
|
|
|
tecs_status.height_rate = -_local_pos.vz;
|
|
|
|
|
tecs_status.equivalent_airspeed_sp = equivalent_airspeed_sp;
|
|
|
|
|
tecs_status.true_airspeed_sp = debug_output.true_airspeed_sp;
|
|
|
|
|
tecs_status.true_airspeed_filtered = debug_output.true_airspeed_filtered;
|
|
|
|
|
tecs_status.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
|
|
|
|
|
tecs_status.true_airspeed_derivative = debug_output.true_airspeed_derivative;
|
|
|
|
|
tecs_status.true_airspeed_derivative_raw = true_airspeed_derivative_raw;
|
|
|
|
|
tecs_status.total_energy_rate = debug_output.control.total_energy_rate_estimate;
|
|
|
|
|
tecs_status.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate;
|
|
|
|
|
tecs_status.total_energy_rate_sp = debug_output.control.total_energy_rate_sp;
|
|
|
|
|
tecs_status.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp;
|
|
|
|
|
tecs_status.throttle_integ = debug_output.control.throttle_integrator;
|
|
|
|
|
tecs_status.pitch_integ = debug_output.control.pitch_integrator;
|
|
|
|
|
tecs_status.throttle_sp = _tecs.get_throttle_setpoint();
|
|
|
|
|
tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint();
|
|
|
|
|
tecs_status.throttle_trim = throttle_trim;
|
|
|
|
|
tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio();
|
|
|
|
|
tecs_status.fast_descend_ratio = debug_output.fast_descend;
|
|
|
|
|
|
|
|
|
|
tecs_status.timestamp = hrt_absolute_time();
|
|
|
|
|
|
|
|
|
|
_tecs_status_pub.publish(tecs_status);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int FwLateralLongitudinalControl::task_spawn(int argc, char *argv[])
|
|
|
|
|
{
|
|
|
|
|
bool is_vtol = false;
|
|
|
|
|
|
|
|
|
|
if (argc > 1) {
|
|
|
|
|
if (strcmp(argv[1], "vtol") == 0) {
|
|
|
|
|
is_vtol = true;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
FwLateralLongitudinalControl *instance = new FwLateralLongitudinalControl(is_vtol);
|
|
|
|
|
|
|
|
|
|
if (instance) {
|
|
|
|
|
_object.store(instance);
|
|
|
|
|
_task_id = task_id_is_work_queue;
|
|
|
|
|
|
|
|
|
|
if (instance->init()) {
|
|
|
|
|
return PX4_OK;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
PX4_ERR("alloc failed");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
delete instance;
|
|
|
|
|
_object.store(nullptr);
|
|
|
|
|
_task_id = -1;
|
|
|
|
|
|
|
|
|
|
return PX4_ERROR;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool
|
|
|
|
|
FwLateralLongitudinalControl::init()
|
|
|
|
|
{
|
|
|
|
|
if (!_local_pos_sub.registerCallback()) {
|
|
|
|
|
PX4_ERR("callback registration failed");
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int FwLateralLongitudinalControl::custom_command(int argc, char *argv[])
|
|
|
|
|
{
|
|
|
|
|
return print_usage("unknown command");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int FwLateralLongitudinalControl::print_usage(const char *reason)
|
|
|
|
|
{
|
|
|
|
|
if (reason) {
|
|
|
|
|
PX4_WARN("%s\n", reason);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
PRINT_MODULE_DESCRIPTION(
|
|
|
|
|
R"DESCR_STR(
|
|
|
|
|
### Description
|
|
|
|
|
fw_lat_lon_control computes attitude and throttle setpoints from lateral and longitudinal control setpoints.
|
|
|
|
|
|
|
|
|
|
)DESCR_STR");
|
|
|
|
|
|
|
|
|
|
PRINT_MODULE_USAGE_NAME("fw_lat_lon_control", "controller");
|
|
|
|
|
PRINT_MODULE_USAGE_COMMAND("start");
|
|
|
|
|
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
|
|
|
|
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
|
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void FwLateralLongitudinalControl::update_control_state() {
|
|
|
|
|
updateAltitudeAndHeightRate();
|
|
|
|
|
updateAirspeed();
|
|
|
|
|
updateAttitude();
|
|
|
|
|
updateWind();
|
|
|
|
|
|
|
|
|
|
_lateral_control_state.ground_speed = Vector2f(_local_pos.vx, _local_pos.vy);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void FwLateralLongitudinalControl::updateWind() {
|
|
|
|
|
if (_wind_sub.updated()) {
|
|
|
|
|
wind_s wind{};
|
|
|
|
|
_wind_sub.update(&wind);
|
|
|
|
|
|
|
|
|
|
// assumes wind is valid if finite
|
|
|
|
|
_wind_valid = PX4_ISFINITE(wind.windspeed_north)
|
|
|
|
|
&& PX4_ISFINITE(wind.windspeed_east);
|
|
|
|
|
|
|
|
|
|
_time_wind_last_received = hrt_absolute_time();
|
|
|
|
|
|
|
|
|
|
_lateral_control_state.wind_speed(0) = wind.windspeed_north;
|
|
|
|
|
_lateral_control_state.wind_speed(1) = wind.windspeed_east;
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
|
|
|
|
|
_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (!_wind_valid) {
|
|
|
|
|
_lateral_control_state.wind_speed.setZero();
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void FwLateralLongitudinalControl::updateAltitudeAndHeightRate() {
|
|
|
|
|
float ref_alt{0.f};
|
|
|
|
|
if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) {
|
|
|
|
|
ref_alt = _local_pos.ref_alt;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_long_control_state.altitude_msl = -_local_pos.z + ref_alt; // Altitude AMSL in meters
|
|
|
|
|
_long_control_state.height_rate = -_local_pos.vz;
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void FwLateralLongitudinalControl::updateAttitude() {
|
|
|
|
|
vehicle_attitude_s att;
|
|
|
|
|
|
|
|
|
|
if (_vehicle_attitude_sub.update(&att)) {
|
|
|
|
|
|
|
|
|
|
Dcmf R{Quatf(att.q)};
|
|
|
|
|
|
|
|
|
|
// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
|
|
|
|
|
// between multirotor and fixed wing flight
|
|
|
|
|
if (_vehicle_status_sub.get().is_vtol_tailsitter) {
|
|
|
|
|
const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}};
|
|
|
|
|
R = R * R_offset;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
const Eulerf euler_angles(R);
|
|
|
|
|
_long_control_state.pitch_rad = euler_angles.theta();
|
|
|
|
|
_yaw = euler_angles.psi();
|
|
|
|
|
|
|
|
|
|
// load factor due to banking
|
|
|
|
|
const float load_factor_from_bank_angle = 1.0f / max(cosf(euler_angles.phi()), FLT_EPSILON);
|
|
|
|
|
_tecs.set_load_factor(load_factor_from_bank_angle);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void FwLateralLongitudinalControl::updateAirspeed() {
|
|
|
|
|
bool airspeed_valid = _airspeed_valid;
|
|
|
|
|
airspeed_validated_s airspeed_validated;
|
|
|
|
|
|
|
|
|
|
if (_param_fw_use_airspd.get() && _airspeed_validated_sub.update(&airspeed_validated)) {
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
|
|
|
|
|
&& PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) {
|
|
|
|
|
|
|
|
|
|
airspeed_valid = true;
|
|
|
|
|
|
|
|
|
|
_time_airspeed_last_valid = airspeed_validated.timestamp;
|
|
|
|
|
_long_control_state.airspeed_eas = airspeed_validated.calibrated_airspeed_m_s;
|
|
|
|
|
|
|
|
|
|
_long_control_state.eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
airspeed_valid = false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
// no airspeed updates for one second
|
|
|
|
|
if (airspeed_valid && (hrt_elapsed_time(&_time_airspeed_last_valid) > 1_s)) {
|
|
|
|
|
airspeed_valid = false;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// update TECS if validity changed
|
|
|
|
|
if (airspeed_valid != _airspeed_valid) {
|
|
|
|
|
_tecs.enable_airspeed(airspeed_valid);
|
|
|
|
|
_airspeed_valid = airspeed_valid;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
bool FwLateralLongitudinalControl::checkLowHeightConditions() const
|
|
|
|
|
{
|
|
|
|
|
// Are conditions for low-height
|
|
|
|
|
return _param_fw_t_thr_low_hgt.get() >= 0.f && _local_pos.dist_bottom_valid
|
|
|
|
|
&& _local_pos.dist_bottom < _param_fw_t_thr_low_hgt.get();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void FwLateralLongitudinalControl::updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt)
|
|
|
|
|
{
|
|
|
|
|
// Target time constant for the TECS altitude tracker
|
|
|
|
|
float alt_tracking_tc = _param_fw_t_h_error_tc.get();
|
|
|
|
|
|
|
|
|
|
if (is_low_height) {
|
|
|
|
|
// If low-height conditions satisfied, compute target time constant for altitude tracking
|
|
|
|
|
alt_tracking_tc *= _param_fw_thrtc_sc.get();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_tecs_alt_time_const_slew_rate.update(alt_tracking_tc, dt);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float FwLateralLongitudinalControl::getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const
|
|
|
|
|
{
|
|
|
|
|
if (is_wind_valid) {
|
|
|
|
|
// If we have a valid wind estimate, npfg is able to handle all degenerated cases
|
|
|
|
|
return 1.f;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// NPFG can run without wind information as long as the system is not flying backwards and has a minimal ground speed
|
|
|
|
|
// Check the minimal ground speed. if it is greater than twice the standard deviation, we assume that we can infer a valid track angle
|
|
|
|
|
const Vector2f ground_vel(local_pos.vx, local_pos.vy);
|
|
|
|
|
const float ground_speed(ground_vel.norm());
|
|
|
|
|
const float low_ground_speed_factor(math::constrain((ground_speed - HORIZONTAL_EVH_FACTOR_COURSE_INVALID *
|
|
|
|
|
local_pos.evh) / ((HORIZONTAL_EVH_FACTOR_COURSE_VALID - HORIZONTAL_EVH_FACTOR_COURSE_INVALID)*local_pos.evh),
|
|
|
|
|
0.f, 1.f));
|
|
|
|
|
|
|
|
|
|
// Check that the angle between heading and track is not off too much. if it is greater than 90° we will be pushed back from the wind and the npfg will propably give a roll command in the wrong direction.
|
|
|
|
|
const Vector2f heading_vector(matrix::Dcm2f(local_pos.heading)*Vector2f({1.f, 0.f}));
|
|
|
|
|
const Vector2f ground_vel_norm(ground_vel.normalized());
|
|
|
|
|
const float flying_forward_factor(math::constrain((heading_vector.dot(ground_vel_norm) -
|
|
|
|
|
COS_HEADING_TRACK_ANGLE_PUSHED_BACK) / ((COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK -
|
|
|
|
|
COS_HEADING_TRACK_ANGLE_PUSHED_BACK)),
|
|
|
|
|
0.f, 1.f));
|
|
|
|
|
|
|
|
|
|
return flying_forward_factor * low_ground_speed_factor;
|
|
|
|
|
}
|
|
|
|
|
float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float lateral_accel_sp)
|
|
|
|
|
{
|
|
|
|
|
// Scale the npfg output to zero if npfg is not certain for correct output
|
|
|
|
|
const float can_run_factor{math::constrain(getGuidanceQualityFactor(_local_pos, _wind_valid), 0.f, 1.f)};
|
|
|
|
|
|
|
|
|
|
hrt_abstime now{hrt_absolute_time()};
|
|
|
|
|
|
|
|
|
|
// Warn the user when the scale is less than 90% for at least 2 seconds (disable in transition)
|
|
|
|
|
|
|
|
|
|
// If the npfg was not running before, reset the user warning variables.
|
|
|
|
|
if ((now - _time_since_last_npfg_call) > ROLL_WARNING_TIMEOUT) {
|
|
|
|
|
_need_report_npfg_uncertain_condition = true;
|
|
|
|
|
_time_since_first_reduced_roll = 0U;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (_vehicle_status_sub.get().in_transition_mode || can_run_factor > ROLL_WARNING_CAN_RUN_THRESHOLD || _landed) {
|
|
|
|
|
// NPFG reports a good condition or we are in transition, reset the user warning variables.
|
|
|
|
|
_need_report_npfg_uncertain_condition = true;
|
|
|
|
|
_time_since_first_reduced_roll = 0U;
|
|
|
|
|
|
|
|
|
|
} else if (_need_report_npfg_uncertain_condition) {
|
|
|
|
|
if (_time_since_first_reduced_roll == 0U) {
|
|
|
|
|
_time_since_first_reduced_roll = now;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
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,
|
|
|
|
|
"Roll command reduced due to uncertain velocity/wind estimates!");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
// Nothing to do, already reported.
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_time_since_last_npfg_call = now;
|
|
|
|
|
|
|
|
|
|
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);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
extern "C" __EXPORT int fw_lat_lon_control_main(int argc, char *argv[])
|
|
|
|
|
{
|
|
|
|
|
return FwLateralLongitudinalControl::main(argc, argv);
|
|
|
|
|
}
|
RomanBapst