diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index c28adef56a..0c02a00c7f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -28,6 +28,7 @@ fi fw_rate_control start vtol fw_att_control start vtol fw_pos_control start vtol +fw_lat_lon_control start vtol fw_autotune_attitude_control start vtol # Start Land Detector diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 1584f93ef6..e091ca76bd 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -21,6 +21,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_FIGURE_OF_EIGHT=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y diff --git a/src/modules/fw_lateral_longitudinal_control/CMakeLists.txt b/src/modules/fw_lateral_longitudinal_control/CMakeLists.txt new file mode 100644 index 0000000000..7642594b7a --- /dev/null +++ b/src/modules/fw_lateral_longitudinal_control/CMakeLists.txt @@ -0,0 +1,15 @@ +set(CONTROL_DEPENDENCIES + npfg + tecs +) + + +px4_add_module( + MODULE modules__fw_lateral_longitudinal_control + MAIN fw_lat_lon_control + SRCS + FwLateralLongitudinalControl.cpp + FwLateralLongitudinalControl.hpp + DEPENDS + ${CONTROL_DEPENDENCIES} +) diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp new file mode 100644 index 0000000000..e3068a8dae --- /dev/null +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -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 + +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); +} diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp new file mode 100644 index 0000000000..fd87c4602b --- /dev/null +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -0,0 +1,230 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + +/** + * @file FwLateralLongitudinalControl.hpp + */ + +#ifndef PX4_FWLATERALLONGITUDINALCONTROL_HPP +#define PX4_FWLATERALLONGITUDINALCONTROL_HPP + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +class FwLateralLongitudinalControl final : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + FwLateralLongitudinalControl(bool is_vtol); + + ~FwLateralLongitudinalControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + uORB::Subscription _wind_sub{ORB_ID(wind)}; + uORB::SubscriptionData _control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::SubscriptionData _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_landed_sub{ORB_ID(vehicle_land_detected)}; + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _fw_lateral_ctrl_sub{ORB_ID(fw_lateral_control_setpoint)}; + uORB::Subscription _fw_longitudinal_ctrl_sub{ORB_ID(fw_longitudinal_control_setpoint)}; + uORB::SubscriptionData _long_control_limits_sub{ORB_ID(longitudinal_control_limits)}; + uORB::SubscriptionData _lateral_control_limits_sub{ORB_ID(lateral_control_limits)}; + + vehicle_local_position_s _local_pos{}; + + uORB::Publication _attitude_sp_pub; + uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; + uORB::PublicationData _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)}; + uORB::PublicationData _lateral_ctrl_status_pub{ORB_ID(fw_lateral_control_status)}; + uORB::PublicationData _longitudinal_ctrl_status_pub{ + ORB_ID(fw_longitudinal_control_status)}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_fw_psp_off, + (ParamBool) _param_fw_use_airspd, + (ParamFloat) _param_nav_fw_alt_rad, + (ParamFloat) _param_fw_r_lim, + (ParamFloat) _param_fw_p_lim_max, + (ParamFloat) _param_fw_p_lim_min, + (ParamFloat) _param_fw_pn_r_slew_max, + (ParamFloat) _param_fw_t_hrate_ff, + (ParamFloat) _param_fw_t_h_error_tc, + (ParamFloat) _param_fw_t_fast_alt_err, + (ParamFloat) _param_fw_t_thr_integ, + (ParamFloat) _param_fw_t_I_gain_pit, + (ParamFloat) _param_fw_t_ptch_damp, + (ParamFloat) _param_fw_t_rll2thr, + (ParamFloat) _param_fw_t_sink_max, + (ParamFloat) _param_fw_t_tas_error_tc, + (ParamFloat) _param_fw_t_thr_damping, + (ParamFloat) _param_fw_t_vert_acc, + (ParamFloat) _param_ste_rate_time_const, + (ParamFloat) _param_seb_rate_ff, + (ParamFloat) _param_speed_standard_dev, + (ParamFloat) _param_speed_rate_standard_dev, + (ParamFloat) _param_process_noise_standard_dev, + + (ParamFloat) _param_fw_thr_slew_max, + (ParamFloat) _param_fw_thrtc_sc, + (ParamFloat) _param_fw_t_thr_low_hgt + ) + + + hrt_abstime _last_time_loop_ran{}; + uint8_t _z_reset_counter{0}; + bool _tecs_is_running{false}; + bool _airspeed_valid{false}; + uint64_t _time_airspeed_last_valid{0}; + float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; + // Smooths changes in the altitude tracking error time constant value + SlewRate _tecs_alt_time_const_slew_rate; + struct longitudinal_control_state { + float pitch_rad{0.f}; + float altitude_msl{0.f}; + float airspeed_eas{0.f}; + float eas2tas{1.f}; + float height_rate{0.f}; + } _long_control_state{}; + + bool _wind_valid{false}; + hrt_abstime _time_wind_last_received{0}; + SlewRate _roll_slew_rate; + float _yaw{0.f}; + struct lateral_control_state { + matrix::Vector2f ground_speed; + matrix::Vector2f wind_speed; + } _lateral_control_state{}; + bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed + hrt_abstime _time_since_first_reduced_roll{0U}; ///< absolute time since start when entering reduced roll angle for the first time + hrt_abstime _time_since_last_npfg_call{0U}; ///< absolute time since start when the npfg reduced roll angle calculations was last performed + vehicle_attitude_setpoint_s _att_sp{}; + + bool _landed{false}; + + perf_counter_t _loop_perf; // loop performance counter + + PerformanceModel _performance_model; + TECS _tecs; + CourseToAirspeedRefMapper _course_to_airspeed; + AirspeedReferenceController _airspeed_ref_control; + + void parameters_update(); + void update_control_state(); + void 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); + + void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw, + float throttle_trim); + + void updateAirspeed(); + + void updateAttitude(); + + void updateAltitudeAndHeightRate(); + + float mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const; + + void updateWind(); + + void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt); + + bool checkLowHeightConditions() const; + + float getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const; + + float getCorrectedLateralAccelSetpoint(float lateral_accel_sp); +}; + + +#endif //PX4_FWLATERALLONGITUDINALCONTROL_HPP diff --git a/src/modules/fw_lateral_longitudinal_control/Kconfig b/src/modules/fw_lateral_longitudinal_control/Kconfig new file mode 100644 index 0000000000..aefa5a7559 --- /dev/null +++ b/src/modules/fw_lateral_longitudinal_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_FW_LATERAL_LONGITUDINAL_CONTROL + bool "fw_lateral_longitudinal_control" + default n + ---help--- + Enable support for fw_lat_lon_control