diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index a15f1e8f71..5490cd0ade 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -48,15 +48,16 @@ using matrix::Vector2d; using matrix::Vector3f; using matrix::wrap_pi; +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, .heading_sp_runway_takeoff = NAN, .reset_integral = false}; +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}; FixedwingPositionControl::FixedwingPositionControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _launchDetector(this), _runway_takeoff(this) #ifdef CONFIG_FIGURE_OF_EIGHT - , _figure_eight(_npfg, _wind_vel, _eas2tas) + , _figure_eight(_directional_guidance, _wind_vel, _eas2tas) #endif // CONFIG_FIGURE_OF_EIGHT { @@ -65,7 +66,6 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : _pos_ctrl_status_pub.advertise(); _pos_ctrl_landing_status_pub.advertise(); - _tecs_status_pub.advertise(); _launch_detection_status_pub.advertise(); _landing_gear_pub.advertise(); @@ -77,11 +77,6 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : /* fetch initial parameter values */ parameters_update(); - _roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get())); - _roll_slew_rate.setForcedValue(0.f); - - _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()); } @@ -108,49 +103,16 @@ FixedwingPositionControl::parameters_update() _performance_model.updateParameters(); - _roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get())); - // NPFG parameters - _npfg.setPeriod(_param_npfg_period.get()); - _npfg.setDamping(_param_npfg_damping.get()); - _npfg.enablePeriodLB(_param_npfg_en_period_lb.get()); - _npfg.enablePeriodUB(_param_npfg_en_period_ub.get()); - _npfg.enableMinGroundSpeed(_param_npfg_en_min_gsp.get()); - _npfg.enableTrackKeeping(_param_npfg_en_track_keeping.get()); - _npfg.enableWindExcessRegulation(_param_npfg_en_wind_reg.get()); - _npfg.setMinGroundSpeed(_param_fw_gnd_spd_min.get()); - _npfg.setMaxTrackKeepingMinGroundSpeed(_param_npfg_track_keeping_gsp_max.get()); - _npfg.setRollTimeConst(_param_npfg_roll_time_const.get()); - _npfg.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get()); - _npfg.setRollLimit(radians(_param_fw_r_lim.get())); - _npfg.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get()); + _directional_guidance.setPeriod(_param_npfg_period.get()); + _directional_guidance.setDamping(_param_npfg_damping.get()); + _directional_guidance.enablePeriodLB(_param_npfg_en_period_lb.get()); + _directional_guidance.enablePeriodUB(_param_npfg_en_period_ub.get()); + _directional_guidance.setRollTimeConst(_param_npfg_roll_time_const.get()); + _directional_guidance.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get()); + _directional_guidance.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get()); // TECS parameters - _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_speed_weight(_param_fw_t_spdweight.get()); - _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(); } void @@ -237,7 +199,6 @@ FixedwingPositionControl::airspeed_poll() // update TECS if validity changed if (airspeed_valid != _airspeed_valid) { - _tecs.enable_airspeed(airspeed_valid); _airspeed_valid = airspeed_valid; } } @@ -318,7 +279,7 @@ FixedwingPositionControl::vehicle_attitude_poll() } const Eulerf euler_angles(R); - _pitch = euler_angles(1); + euler_angles(1); _yaw = euler_angles(2); Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; @@ -328,7 +289,6 @@ FixedwingPositionControl::vehicle_attitude_poll() _body_velocity_x = body_velocity(0); // load factor due to banking - _tecs.set_load_factor(getLoadFactor()); } } @@ -390,8 +350,7 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, _performance_model.getMaximumCalibratedAirspeed()); // initialize the airspeed setpoint to the max(current airsped, min airspeed) - if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) - || !_tecs_is_running) { + if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) { _airspeed_slew_rate_controller.setForcedValue(math::max(calibrated_min_airspeed, _airspeed_eas)); } @@ -412,90 +371,6 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, return _airspeed_slew_rate_controller.getState(); } -void -FixedwingPositionControl::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); -} - -void -FixedwingPositionControl::status_publish() -{ - position_controller_status_s pos_ctrl_status = {}; - - npfg_status_s npfg_status = {}; - - npfg_status.wind_est_valid = _wind_valid; - - const float bearing = _npfg.getBearing(); // dont repeat atan2 calc - - pos_ctrl_status.nav_bearing = bearing; - pos_ctrl_status.target_bearing = _target_bearing; - pos_ctrl_status.xtrack_error = _npfg.getTrackError(); - pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f); - - npfg_status.lat_accel = _npfg.getLateralAccel(); - npfg_status.lat_accel_ff = _npfg.getLateralAccelFF(); - npfg_status.heading_ref = _npfg.getHeadingRef(); - npfg_status.bearing = bearing; - npfg_status.bearing_feas = _npfg.getBearingFeas(); - npfg_status.bearing_feas_on_track = _npfg.getOnTrackBearingFeas(); - npfg_status.signed_track_error = _npfg.getTrackError(); - npfg_status.track_error_bound = _npfg.getTrackErrorBound(); - npfg_status.airspeed_ref = _npfg.getAirspeedRef(); - npfg_status.min_ground_speed_ref = _npfg.getMinGroundSpeedRef(); - npfg_status.adapted_period = _npfg.getAdaptedPeriod(); - npfg_status.p_gain = _npfg.getPGain(); - npfg_status.time_const = _npfg.getTimeConst(); - npfg_status.can_run_factor = _npfg.canRun(_local_pos, _wind_valid); - npfg_status.timestamp = hrt_absolute_time(); - - _npfg_status_pub.publish(npfg_status); - - pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); - - pos_ctrl_status.yaw_acceptance = NAN; - - pos_ctrl_status.timestamp = hrt_absolute_time(); - - pos_ctrl_status.type = _position_sp_type; - - _pos_ctrl_status_pub.publish(pos_ctrl_status); -} - void FixedwingPositionControl::landing_status_publish() { @@ -509,47 +384,6 @@ FixedwingPositionControl::landing_status_publish() _pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status); } -float FixedwingPositionControl::getCorrectedNpfgRollSetpoint() -{ - // Scale the npfg output to zero if npfg is not certain for correct output - float new_roll_setpoint(_npfg.getRollSetpoint()); - const float can_run_factor(constrain(_npfg.canRun(_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.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 * (new_roll_setpoint); -} - void FixedwingPositionControl::updateLandingAbortStatus(const uint8_t new_abort_status) { @@ -591,41 +425,6 @@ FixedwingPositionControl::updateLandingAbortStatus(const uint8_t new_abort_statu } } -void -FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev, - position_setpoint_s &waypoint_next, bool flag_init) -{ - position_setpoint_s temp_prev = waypoint_prev; - position_setpoint_s temp_next = waypoint_next; - - if (flag_init) { - // previous waypoint: HDG_HOLD_SET_BACK_DIST meters behind us - waypoint_from_heading_and_distance(_current_latitude, _current_longitude, heading + radians(180.0f), - HDG_HOLD_SET_BACK_DIST, &temp_prev.lat, &temp_prev.lon); - - // next waypoint: HDG_HOLD_DIST_NEXT meters in front of us - waypoint_from_heading_and_distance(_current_latitude, _current_longitude, heading, - HDG_HOLD_DIST_NEXT, &temp_next.lat, &temp_next.lon); - - } else { - // use the existing flight path from prev to next - - // previous waypoint: shifted HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST - create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon, - HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST, &temp_prev.lat, &temp_prev.lon); - - // next waypoint: shifted -(HDG_HOLD_DIST_NEXT + HDG_HOLD_REACHED_DIST) - create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon, - -(HDG_HOLD_REACHED_DIST + HDG_HOLD_DIST_NEXT), &temp_next.lat, &temp_next.lon); - } - - waypoint_prev = temp_prev; - waypoint_prev.alt = _current_altitude; - - waypoint_next = temp_next; - waypoint_next.alt = _current_altitude; -} - float FixedwingPositionControl::getManualHeightRateSetpoint() { @@ -884,15 +683,10 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto } } + switch (position_sp_type) { case position_setpoint_s::SETPOINT_TYPE_IDLE: { - _att_sp.thrust_body[0] = 0.0f; - const float roll_body = 0.0f; - const float pitch_body = radians(_param_fw_psp_off.get()); - const float yaw_body = 0.0f; - - const Quatf setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - setpoint.copyTo(_att_sp.q_d); + control_idle(); break; } @@ -930,54 +724,89 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto #endif // CONFIG_FIGURE_OF_EIGHT - /* Copy thrust output for publication, handle special cases */ - if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { - - _att_sp.thrust_body[0] = 0.0f; - - } else { - // when we are landed state we want the motor to spin at idle speed - _att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust(); - } if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(current_sp); } } +void FixedwingPositionControl::control_idle() +{ + const hrt_abstime now = hrt_absolute_time(); + fw_lateral_control_setpoint_s lateral_ctrl_sp {empty_lateral_control_setpoint}; + lateral_ctrl_sp.timestamp = now; + lateral_ctrl_sp.lateral_acceleration_setpoint = 0.0f; + _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); + + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); + + fw_longitudinal_control_setpoint_s long_contrl_sp {empty_longitudinal_control_setpoint}; + long_contrl_sp.timestamp = now; + long_contrl_sp.pitch_sp = math::radians(_param_fw_psp_off.get()); + long_contrl_sp.thrust_sp = 0.0f; + _longitudinal_ctrl_sp_pub.publish(long_contrl_sp); + + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = now}; + setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits.throttle_max = 0.f; + longitudinal_control_limits.throttle_min = 0.f; + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); +} + +void FixedwingPositionControl::setDefaultLongControlLimits(longitudinal_control_limits_s &longitudinal_control_limits) +const +{ + longitudinal_control_limits.equivalent_airspeed_min = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); + longitudinal_control_limits.equivalent_airspeed_max = _performance_model.getMaximumCalibratedAirspeed(); + longitudinal_control_limits.pitch_min = radians(_param_fw_p_lim_min.get()); + longitudinal_control_limits.pitch_max = radians(_param_fw_p_lim_max.get()); + longitudinal_control_limits.throttle_min = _param_fw_thr_min.get(); + longitudinal_control_limits.throttle_max = _param_fw_thr_max.get(); + longitudinal_control_limits.climb_rate_max = _param_climbrate_target.get(); + longitudinal_control_limits.sink_rate_max = _param_sinkrate_target.get(); + longitudinal_control_limits.disable_underspeed_protection = false; + longitudinal_control_limits.enforce_low_height_condition = false; + longitudinal_control_limits.speed_weight = _param_fw_t_spdweight.get(); +} + void FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval) { - const bool is_low_height = checkLowHeightConditions(); + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = NAN, + .altitude_setpoint = _current_altitude, + .equivalent_airspeed_setpoint = _performance_model.getCalibratedTrimAirspeed(), + .pitch_sp = NAN, + .thrust_sp = NAN + }; - // only control altitude and airspeed ("fixed-bank loiter") - tecs_update_pitch_throttle(control_interval, - _current_altitude, - _performance_model.getCalibratedTrimAirspeed(), - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - const float yaw_body = 0.f; + float throttle_max = _param_fw_thr_max.get(); // Special case: if z or vz estimate is invalid we cannot control height anymore. To prevent a // "climb-away" we set the thrust to MIN in that case. if (_landed || !_local_pos.z_valid || !_local_pos.v_z_valid) { - _att_sp.thrust_body[0] = _param_fw_thr_min.get(); - - } else { - _att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get()); + throttle_max = _param_fw_thr_min.get(); } - const float pitch_body = get_tecs_pitch(); - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits.throttle_max = throttle_max; + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); + fw_lateral_control_setpoint_s lateral_ctrl_sp = empty_lateral_control_setpoint; + lateral_ctrl_sp.timestamp = hrt_absolute_time(); + const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + lateral_ctrl_sp.lateral_acceleration_setpoint = rollAngleToLateralAccel(roll_body); + _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); + + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); } void @@ -985,35 +814,35 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) { // Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover, // but not letting it drift too far away. - const float descend_rate = -0.5f; - const bool disable_underspeed_handling = false; + const float descend_rate = 0.5f; - const bool is_low_height = checkLowHeightConditions(); + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = -descend_rate, + .altitude_setpoint = _current_altitude, + .equivalent_airspeed_setpoint = _performance_model.getCalibratedTrimAirspeed(), + .pitch_sp = NAN, + .thrust_sp = NAN + }; - tecs_update_pitch_throttle(control_interval, - _current_altitude, - _performance_model.getCalibratedTrimAirspeed(), - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - descend_rate); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits.throttle_max = (_landed + || !_local_pos.v_z_valid) ? _param_fw_thr_min.get() : _param_fw_thr_max.get(); + + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); + + fw_lateral_control_setpoint_s lateral_ctrl_sp = empty_lateral_control_setpoint; + lateral_ctrl_sp.timestamp = hrt_absolute_time(); const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - const float yaw_body = 0.f; + lateral_ctrl_sp.lateral_acceleration_setpoint = rollAngleToLateralAccel(roll_body); + _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); - // Special case: if vz estimate is invalid we cannot control height rate anymore. To prevent a - // "climb-away" we set the thrust to MIN in that case. - _att_sp.thrust_body[0] = (_landed - || !_local_pos.v_z_valid) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - - const float pitch_body = get_tecs_pitch(); - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); } uint8_t @@ -1031,7 +860,7 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp /* current waypoint (the one currently heading for) */ curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - const float acc_rad = _npfg.switchDistance(500.0f); + const float acc_rad = _directional_guidance.switchDistance(500.0f); const bool approaching_vtol_backtransition = _vehicle_status.is_vtol && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _position_setpoint_current_valid @@ -1072,19 +901,14 @@ void FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { - const float acc_rad = _npfg.switchDistance(500.0f); - float tecs_fw_thr_min; - 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) { /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; - - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); + throttle_min = 0.0; + throttle_max = 0.0; } // waypoint is a plain navigation waypoint @@ -1122,103 +946,97 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co } } - float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); + const float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); + + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = NAN, + .altitude_setpoint = position_sp_alt, + .equivalent_airspeed_setpoint = target_airspeed, + .pitch_sp = NAN, + .thrust_sp = NAN + }; + + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits.throttle_min = throttle_min; + longitudinal_control_limits.throttle_max = throttle_max; + longitudinal_control_limits.speed_weight = pos_sp_curr.gliding_enabled ? 2.0f : + longitudinal_control_limits.speed_weight; + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); + DirectionalGuidanceOutput sp{}; if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { Vector2f prev_wp_local = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon); - navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + sp = navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } else { - navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + sp = navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + fw_lateral_control_setpoint_s lateral_ctrl_sp{empty_lateral_control_setpoint}; + lateral_ctrl_sp.timestamp = hrt_absolute_time(); + lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - const bool is_low_height = checkLowHeightConditions(); - - tecs_update_pitch_throttle(control_interval, - position_sp_alt, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - tecs_fw_thr_min, - tecs_fw_thr_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); - - const float pitch_body = get_tecs_pitch(); - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); } void FixedwingPositionControl::control_auto_velocity(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { - float tecs_fw_thr_min; - float tecs_fw_thr_max; - - if (pos_sp_curr.gliding_enabled) { - /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; - - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); - } - - // waypoint is a plain navigation waypoint - float position_sp_alt = pos_sp_curr.alt; - - //Offboard velocity control Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy}; - _target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0))); + const float target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0))); float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel); - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + const Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + const DirectionalGuidanceOutput sp = navigateBearing(curr_pos_local, target_bearing, ground_speed, _wind_vel); - float yaw_body = _yaw; - const bool disable_underspeed_handling = false; + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - const bool is_low_height = checkLowHeightConditions(); + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); - tecs_update_pitch_throttle(control_interval, - position_sp_alt, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - tecs_fw_thr_min, - tecs_fw_thr_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - pos_sp_curr.vz); - const float pitch_body = get_tecs_pitch(); + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = pos_sp_curr.vz, + .altitude_setpoint = pos_sp_curr.alt, + .equivalent_airspeed_setpoint = target_airspeed, + .pitch_sp = NAN, + .thrust_sp = NAN + }; - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); + + if (pos_sp_curr.gliding_enabled) { + longitudinal_control_limits.throttle_min = 0.f; + longitudinal_control_limits.throttle_max = 0.f; + longitudinal_control_limits.speed_weight = 2.0f; + } + + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); } void @@ -1250,20 +1068,6 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons airspeed_sp = pos_sp_curr.cruising_speed; } - float tecs_fw_thr_min; - float tecs_fw_thr_max; - - if (pos_sp_curr.gliding_enabled) { - /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; - - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); - } - /* waypoint is a loiter waypoint */ float loiter_radius = pos_sp_curr.loiter_radius; @@ -1275,19 +1079,17 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons Vector2f curr_wp_local{_global_local_proj_ref.project(curr_wp(0), curr_wp(1))}; Vector2f vehicle_to_loiter_center{curr_wp_local - curr_pos_local}; - bool is_low_height = checkLowHeightConditions(); + const bool close_to_circle = vehicle_to_loiter_center.norm() < loiter_radius + _directional_guidance.switchDistance( + 500); - const bool close_to_circle = vehicle_to_loiter_center.norm() < loiter_radius + _npfg.switchDistance(500); + bool enforce_low_height{false}; if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid && close_to_circle && _param_fw_lnd_earlycfg.get()) { // We're in a loiter directly before a landing WP. Enable our landing configuration (flaps, // landing airspeed and potentially tighter altitude control) already such that we don't // have to do this switch (which can cause significant altitude errors) close to the ground. - - // Just before landing, enforce low-height flight conditions for tighter altitude control - is_low_height = true; - + enforce_low_height = true; airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); @@ -1299,17 +1101,20 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, - ground_speed, - _wind_vel); - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + const DirectionalGuidanceOutput sp = navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, + pos_sp_curr.loiter_direction_counter_clockwise, + ground_speed, + _wind_vel); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; - float alt_sp = pos_sp_curr.alt; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); if (_landing_abort_status) { if (pos_sp_curr.alt - _current_altitude < kClearanceAltitudeBuffer) { @@ -1318,27 +1123,36 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons } else { // continue straight until vehicle has sufficient altitude - roll_body = 0.0f; + lateral_limits.lateral_accel_max = 0.0f; } - is_low_height = true; // In low-height flight, TECS will control altitude tighter + enforce_low_height = true; } - tecs_update_pitch_throttle(control_interval, - alt_sp, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - tecs_fw_thr_min, - tecs_fw_thr_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); + _lateral_ctrl_limits_pub.publish(lateral_limits); - const float pitch_body = get_tecs_pitch(); + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = NAN, + .altitude_setpoint = pos_sp_curr.alt, + .equivalent_airspeed_setpoint = target_airspeed, + .pitch_sp = NAN, + .thrust_sp = NAN + }; - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits.enforce_low_height_condition = enforce_low_height; + + if (pos_sp_curr.gliding_enabled) { + longitudinal_control_limits.throttle_min = 0.f; + longitudinal_control_limits.throttle_max = 0.f; + longitudinal_control_limits.speed_weight = 2.0f; + } + + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); } #ifdef CONFIG_FIGURE_OF_EIGHT @@ -1362,46 +1176,43 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c params.loiter_radius = pos_sp_curr.loiter_radius; // Apply control - _figure_eight.updateSetpoint(curr_pos_local, ground_speed, params, target_airspeed); - float roll_body = _figure_eight.getRollSetpoint(); - target_airspeed = _figure_eight.getAirspeedSetpoint(); - _target_bearing = _figure_eight.getTargetBearing(); + const DirectionalGuidanceOutput sp = _figure_eight.updateSetpoint(curr_pos_local, ground_speed, params, + target_airspeed); + + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; + + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); + _closest_point_on_path = _figure_eight.getClosestPoint(); - // TECS - float tecs_fw_thr_min; - float tecs_fw_thr_max; + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = NAN, + .altitude_setpoint = pos_sp_curr.alt, + .equivalent_airspeed_setpoint = target_airspeed, + .pitch_sp = NAN, + .thrust_sp = NAN + }; + + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); if (pos_sp_curr.gliding_enabled) { - /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; - - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); + longitudinal_control_limits.throttle_min = 0.f; + longitudinal_control_limits.throttle_max = 0.f; + longitudinal_control_limits.speed_weight = 2.0f; } - const bool is_low_height = checkLowHeightConditions(); - - tecs_update_pitch_throttle(control_interval, - pos_sp_curr.alt, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - tecs_fw_thr_min, - tecs_fw_thr_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); - - // Yaw - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - const float pitch_body = get_tecs_pitch(); - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); } void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp) @@ -1424,20 +1235,6 @@ void FixedwingPositionControl::control_auto_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { - float tecs_fw_thr_min; - float tecs_fw_thr_max; - - if (pos_sp_curr.gliding_enabled) { - /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; - - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); - } - // waypoint is a plain navigation waypoint float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); @@ -1445,39 +1242,46 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); // Navigate directly on position setpoint and path tangent matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy); const float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius : 0.0f; - navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); + const DirectionalGuidanceOutput sp = navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), + ground_speed, _wind_vel, curvature); - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); - const bool is_low_height = checkLowHeightConditions(); + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = NAN, + .altitude_setpoint = pos_sp_curr.alt, + .equivalent_airspeed_setpoint = target_airspeed, + .pitch_sp = NAN, + .thrust_sp = NAN + }; - tecs_update_pitch_throttle(control_interval, - pos_sp_curr.alt, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - tecs_fw_thr_min, - tecs_fw_thr_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - _att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max); - const float pitch_body = get_tecs_pitch(); + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + if (pos_sp_curr.gliding_enabled) { + longitudinal_control_limits.throttle_min = 0.f; + longitudinal_control_limits.throttle_max = 0.f; + longitudinal_control_limits.speed_weight = 2.0f; + } + + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); } void @@ -1503,14 +1307,15 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); + if (takeoff_airspeed < adjusted_min_airspeed) { // adjust underspeed detection bounds for takeoff airspeed - _tecs.set_equivalent_airspeed_min(takeoff_airspeed); + longitudinal_control_limits.equivalent_airspeed_min = takeoff_airspeed; adjusted_min_airspeed = takeoff_airspeed; } - const bool is_low_height = checkLowHeightConditions(); - if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { _runway_takeoff.init(now, _yaw, global_position); @@ -1538,7 +1343,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo // tune up the lateral position control guidance when on the ground if (_runway_takeoff.controlYaw()) { - _npfg.setPeriod(_param_rwto_npfg_period.get()); + _directional_guidance.setPeriod(_param_rwto_npfg_period.get()); } @@ -1558,59 +1363,51 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } } - float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, ground_speed, - true); + const float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, + ground_speed, + true); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); + const DirectionalGuidanceOutput sp = navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, + _wind_vel); - float roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; + fw_lateral_ctrl_sp.reset_integral = _runway_takeoff.resetIntegrators(); + fw_lateral_ctrl_sp.heading_sp_runway_takeoff = _runway_takeoff.controlYaw() ? _runway_takeoff.getYaw( + sp.course_setpoint) : NAN; - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + // this overrides the roll setpoint, until the vehicle starts the climbout + fw_lateral_ctrl_sp.roll_sp = _runway_takeoff.getRoll(); + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - // use npfg's bearing to commanded course, controlled via yaw angle while on runway - const float bearing = _npfg.getBearing(); - - // heading hold mode will override this bearing setpoint - float yaw_body = _runway_takeoff.getYaw(bearing); + const float roll_wingtip_strike = getMaxRollAngleNearGround(_current_altitude, _takeoff_ground_alt); + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(roll_wingtip_strike); + _lateral_ctrl_limits_pub.publish(lateral_limits); // update tecs const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get())); const float pitch_min = _runway_takeoff.getMinPitch(math::radians(_takeoff_pitch_min.get()), math::radians(_param_fw_p_lim_min.get())); - if (_runway_takeoff.resetIntegrators()) { - // reset integrals except yaw (which also counts for the wheel controller) - _att_sp.reset_integral = true; - // throttle is open loop anyway during ground roll, no need to wind up the integrator - _tecs.resetIntegrals(); - } + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = NAN, + .altitude_setpoint = altitude_setpoint_amsl, + .equivalent_airspeed_setpoint = target_airspeed, + .pitch_sp = _runway_takeoff.getPitch(), + .thrust_sp = _runway_takeoff.getThrottle(_param_fw_thr_idle.get()) + }; - const bool disable_underspeed_handling = true; + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - tecs_update_pitch_throttle(control_interval, - altitude_setpoint_amsl, - target_airspeed, - pitch_min, - pitch_max, - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density), - is_low_height, - disable_underspeed_handling); - - _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - - const float pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); - _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust()); - - roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + longitudinal_control_limits.pitch_min = pitch_min; + longitudinal_control_limits.pitch_max = pitch_max; + longitudinal_control_limits.climb_rate_max = _performance_model.getMaximumClimbRate(_air_density); + longitudinal_control_limits.disable_underspeed_protection = true; _flaps_setpoint = _param_fw_flaps_to_scl.get(); @@ -1667,60 +1464,57 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, ground_speed, true); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + const DirectionalGuidanceOutput sp = navigateLine(launch_local_position, takeoff_bearing, local_2D_position, + ground_speed, + _wind_vel); + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + + const float roll_wingtip_strike = getMaxRollAngleNearGround(_current_altitude, _takeoff_ground_alt); + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(roll_wingtip_strike); + _lateral_ctrl_limits_pub.publish(lateral_limits); const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); - const bool disable_underspeed_handling = true; + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = NAN, + .altitude_setpoint = altitude_setpoint_amsl, + .equivalent_airspeed_setpoint = target_airspeed, + .pitch_sp = NAN, + .thrust_sp = NAN + }; - tecs_update_pitch_throttle(control_interval, - altitude_setpoint_amsl, - target_airspeed, - radians(_takeoff_pitch_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - max_takeoff_throttle, - _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density), - is_low_height, - disable_underspeed_handling); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { - // explicitly set idle throttle until motors are enabled - _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); + longitudinal_control_limits.pitch_min = radians(_takeoff_pitch_min.get()); + longitudinal_control_limits.throttle_max = max_takeoff_throttle; + longitudinal_control_limits.climb_rate_max = _performance_model.getMaximumClimbRate(_air_density); + longitudinal_control_limits.disable_underspeed_protection = true; - } else { - _att_sp.thrust_body[0] = get_tecs_thrust(); - } - - float pitch_body = get_tecs_pitch(); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); - - Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + //float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } else { + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = 0.f; /* Tell the attitude controller to stop integrating while we are waiting for the launch */ - _att_sp.reset_integral = true; - - /* Set default roll and pitch setpoints during detection phase */ - float roll_body = 0.0f; - float yaw_body = _yaw; - _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); - float pitch_body = radians(_takeoff_pitch_min.get()); - roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); - Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + fw_lateral_ctrl_sp.reset_integral = true; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + fw_longitudinal_control_setpoint_s long_control_sp{empty_longitudinal_control_setpoint}; + long_control_sp.timestamp = hrt_absolute_time(); + long_control_sp.pitch_sp = radians(_takeoff_pitch_min.get()); + _longitudinal_ctrl_sp_pub.publish(long_control_sp); } + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); + launch_detection_status_s launch_detection_status; launch_detection_status.timestamp = now; launch_detection_status.launch_detection_state = _launchDetector.getLaunchDetected(); @@ -1743,14 +1537,18 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits.enforce_low_height_condition = true; + if (airspeed_land < adjusted_min_airspeed) { // adjust underspeed detection bounds for landing airspeed - _tecs.set_equivalent_airspeed_min(airspeed_land); adjusted_min_airspeed = airspeed_land; + longitudinal_control_limits.equivalent_airspeed_min = airspeed_land; } - float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed, - ground_speed); + const float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed, + ground_speed); // now handle position @@ -1799,8 +1597,6 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, // flare at the maximum of the altitude determined by the time before touchdown and a minimum flare altitude const float flare_rel_alt = math::max(_param_fw_lnd_fl_time.get() * _local_pos.vz, _param_fw_lnd_flalt.get()); - // During landing, enforce low-height flight conditions for tighter altitude control - const bool is_low_height = true; // the terrain estimate (if enabled) is always used to determine the flaring altitude if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) { @@ -1826,18 +1622,24 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, // tune up the lateral position control guidance when on the ground const float ground_roll_npfg_period = flare_ramp_interpolator * _param_rwto_npfg_period.get() + (1.0f - flare_ramp_interpolator) * _param_npfg_period.get(); - _npfg.setPeriod(ground_roll_npfg_period); + _directional_guidance.setPeriod(ground_roll_npfg_period); const Vector2f local_approach_entrance = local_land_point - landing_approach_vector; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - float roll_body = getCorrectedNpfgRollSetpoint(); + const DirectionalGuidanceOutput sp = navigateLine(local_approach_entrance, local_land_point, local_position, + ground_speed, + _wind_vel); + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; + fw_lateral_ctrl_sp.heading_sp_runway_takeoff = sp.course_setpoint; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - // use npfg's bearing to commanded course, controlled via yaw angle while on runway - float yaw_body = _npfg.getBearing(); + const float roll_wingtip_strike = getMaxRollAngleNearGround(_current_altitude, terrain_alt); + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(roll_wingtip_strike); + _lateral_ctrl_limits_pub.publish(lateral_limits); /* longitudinal guidance */ @@ -1867,31 +1669,23 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); - const bool disable_underspeed_handling = true; - tecs_update_pitch_throttle(control_interval, - altitude_setpoint, - target_airspeed, - pitch_min_rad, - pitch_max_rad, - _param_fw_thr_idle.get(), - throttle_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - height_rate_setpoint); + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = height_rate_setpoint, + .altitude_setpoint = altitude_setpoint, + .equivalent_airspeed_setpoint = target_airspeed, + .pitch_sp = NAN, + .thrust_sp = NAN + }; - /* set the attitude and throttle commands */ + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - float pitch_body = get_tecs_pitch(); - - roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); - - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + longitudinal_control_limits.pitch_min = pitch_min_rad; + longitudinal_control_limits.pitch_max = pitch_max_rad; + longitudinal_control_limits.throttle_min = _param_fw_thr_idle.get(); + longitudinal_control_limits.throttle_max = throttle_max; + longitudinal_control_limits.disable_underspeed_protection = true; // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = true; @@ -1904,10 +1698,10 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, // blend the height rate controlled throttle setpoints with initial throttle setting over the flare // ramp time period to maintain throttle command continuity when switching from altitude to height rate // control - const float blended_throttle = flare_ramp_interpolator * get_tecs_thrust() + (1.0f - flare_ramp_interpolator) * - _flare_states.initial_throttle_setpoint; - - _att_sp.thrust_body[0] = blended_throttle; +// const float blended_throttle = flare_ramp_interpolator * get_tecs_thrust() + (1.0f - flare_ramp_interpolator) * +// _flare_states.initial_throttle_setpoint; +// +// _att_sp.thrust_body[0] = blended_throttle; } else { @@ -1917,11 +1711,18 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const Vector2f local_approach_entrance = local_land_point - landing_approach_vector; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - float roll_body = getCorrectedNpfgRollSetpoint(); + const DirectionalGuidanceOutput sp = navigateLine(local_approach_entrance, local_land_point, local_position, + ground_speed, + _wind_vel); + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(getMaxRollAngleNearGround(_current_altitude, terrain_alt)); + _lateral_ctrl_limits_pub.publish(lateral_limits); /* longitudinal guidance */ @@ -1931,36 +1732,26 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()), _param_fw_t_sink_max.get()); - tecs_update_pitch_throttle(control_interval, - altitude_setpoint, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - desired_max_sinkrate, - _param_climbrate_target.get(), - is_low_height); + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = NAN, + .altitude_setpoint = altitude_setpoint, + .equivalent_airspeed_setpoint = target_airspeed, + .pitch_sp = NAN, + .thrust_sp = NAN + }; - /* set the attitude and throttle commands */ + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - float pitch_body = get_tecs_pitch(); - - roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); - - // yaw is not controlled in nominal flight - float yaw_body = _yaw; - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + longitudinal_control_limits.throttle_min = _param_fw_thr_idle.get(); + longitudinal_control_limits.throttle_max = _landed ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); + longitudinal_control_limits.sink_rate_max = desired_max_sinkrate; // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = false; - - _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : get_tecs_thrust(); } - _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -1984,14 +1775,17 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); + if (airspeed_land < adjusted_min_airspeed) { // adjust underspeed detection bounds for landing airspeed - _tecs.set_equivalent_airspeed_min(airspeed_land); adjusted_min_airspeed = airspeed_land; + longitudinal_control_limits.equivalent_airspeed_min = airspeed_land; } - float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed, - ground_speed); + const float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed, + ground_speed); const Vector2f local_position{_local_pos.x, _local_pos.y}; @@ -2016,14 +1810,6 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, loiter_radius = _param_nav_loiter_rad.get(); } - // During landing, enforce low-height flight conditions for tighter altitude control - const bool is_low_height = true; - - // the terrain estimate (if enabled) is always used to determine the flaring altitude - float roll_body; - float yaw_body; - float pitch_body; - if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) { // flare and land with minimal speed @@ -2048,18 +1834,18 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float ground_roll_npfg_period = flare_ramp_interpolator * _param_rwto_npfg_period.get() + (1.0f - flare_ramp_interpolator) * _param_npfg_period.get(); - _npfg.setPeriod(ground_roll_npfg_period); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); + _directional_guidance.setPeriod(ground_roll_npfg_period); - navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, - pos_sp_curr.loiter_direction_counter_clockwise, - ground_speed, _wind_vel); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - roll_body = getCorrectedNpfgRollSetpoint(); - - yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, + pos_sp_curr.loiter_direction_counter_clockwise, + ground_speed, _wind_vel); + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; + fw_lateral_ctrl_sp.heading_sp_runway_takeoff = sp.course_setpoint; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); /* longitudinal guidance */ const float flare_ramp_interpolator_sqrt = sqrtf(flare_ramp_interpolator); @@ -2087,28 +1873,22 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); - const bool disable_underspeed_handling = true; + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = height_rate_setpoint, + .altitude_setpoint = _current_altitude, + .equivalent_airspeed_setpoint = target_airspeed, + .pitch_sp = NAN, + .thrust_sp = NAN + }; - tecs_update_pitch_throttle(control_interval, - _current_altitude, // is not controlled, control descend rate - target_airspeed, - pitch_min_rad, - pitch_max_rad, - _param_fw_thr_idle.get(), - throttle_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - height_rate_setpoint); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - /* set the attitude and throttle commands */ - - // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - pitch_body = get_tecs_pitch(); - - // enable direct yaw control using rudder/wheel - _att_sp.fw_control_yaw_wheel = true; + longitudinal_control_limits.pitch_min = pitch_min_rad; + longitudinal_control_limits.pitch_max = pitch_max_rad; + longitudinal_control_limits.throttle_min = _param_fw_thr_idle.get(); + longitudinal_control_limits.throttle_max = throttle_max; + longitudinal_control_limits.disable_underspeed_protection = true; // XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface. if (_param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled) { @@ -2118,24 +1898,23 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, // blend the height rate controlled throttle setpoints with initial throttle setting over the flare // ramp time period to maintain throttle command continuity when switching from altitude to height rate // control - const float blended_throttle = flare_ramp_interpolator * get_tecs_thrust() + (1.0f - flare_ramp_interpolator) * - _flare_states.initial_throttle_setpoint; - - _att_sp.thrust_body[0] = blended_throttle; +// const float blended_throttle = flare_ramp_interpolator * get_tecs_thrust() + (1.0f - flare_ramp_interpolator) * +// _flare_states.initial_throttle_setpoint; +// +// _att_sp.thrust_body[0] = blended_throttle; } else { // follow the glide slope + const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, + pos_sp_curr.loiter_direction_counter_clockwise, + ground_speed, _wind_vel); + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; - /* lateral guidance */ - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - - navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, - pos_sp_curr.loiter_direction_counter_clockwise, - ground_speed, _wind_vel); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - roll_body = getCorrectedNpfgRollSetpoint(); + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); /* longitudinal guidance */ @@ -2145,41 +1924,29 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float glide_slope_sink_rate = airspeed_land * glide_slope / sqrtf(glide_slope * glide_slope + 1.0f); const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()), _param_fw_t_sink_max.get()); - const bool disable_underspeed_handling = false; - tecs_update_pitch_throttle(control_interval, - _current_altitude, // is not controlled, control descend rate - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - desired_max_sinkrate, - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - -glide_slope_sink_rate); // heightrate = -sinkrate + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = -glide_slope_sink_rate, + .altitude_setpoint = _current_altitude, + .equivalent_airspeed_setpoint = target_airspeed, + .pitch_sp = NAN, + .thrust_sp = NAN + }; - /* set the attitude and throttle commands */ + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - pitch_body = get_tecs_pitch(); - - // yaw is not controlled in nominal flight - yaw_body = _yaw; - - // enable direct yaw control using rudder/wheel - _att_sp.fw_control_yaw_wheel = false; - - _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : get_tecs_thrust(); + longitudinal_control_limits.throttle_min = _param_fw_thr_idle.get(); + longitudinal_control_limits.throttle_max = _landed ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); + longitudinal_control_limits.sink_rate_max = desired_max_sinkrate; + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); } - _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - - roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); - - Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(getMaxRollAngleNearGround(_current_altitude, terrain_alt)); + _lateral_ctrl_limits_pub.publish(lateral_limits); _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -2214,32 +1981,34 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max = 0.0f; } + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = height_rate_sp, + .altitude_setpoint = _current_altitude, + .equivalent_airspeed_setpoint = calibrated_airspeed_sp, + .pitch_sp = NAN, + .thrust_sp = NAN + }; - const bool is_low_height = checkLowHeightConditions(); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - const bool disable_underspeed_handling = false; + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits.pitch_min = min_pitch; + longitudinal_control_limits.throttle_max = throttle_max; + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); - tecs_update_pitch_throttle(control_interval, - _current_altitude, - calibrated_airspeed_sp, - min_pitch, - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - throttle_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - height_rate_sp); + const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + const DirectionalGuidanceOutput sp = {.lateral_acceleration_feedforward = rollAngleToLateralAccel(roll_body)}; + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); - const float pitch_body = get_tecs_pitch(); - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); } void @@ -2248,8 +2017,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval, { updateManualTakeoffStatus(); - float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(), - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed, !_completed_manual_takeoff); + const float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(), + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed, !_completed_manual_takeoff); const float height_rate_sp = getManualHeightRateSetpoint(); // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are @@ -2268,11 +2037,6 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _time_last_xy_reset = _local_pos.timestamp; } - Eulerf current_setpoint(Quatf(_att_sp.q_d)); - float yaw_body = current_setpoint.psi(); - float roll_body = current_setpoint.phi(); - float pitch_body = current_setpoint.theta(); - /* heading control */ // TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here) if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH && @@ -2310,33 +2074,33 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _hdg_hold_position = curr_pos_local; } - _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); - roll_body = getCorrectedNpfgRollSetpoint(); - calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; + const DirectionalGuidanceOutput sp = navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, + _wind_vel); + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; - yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); } } - const bool is_low_height = checkLowHeightConditions(); + const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .height_rate_setpoint = height_rate_sp, + .altitude_setpoint = _current_altitude, + .equivalent_airspeed_setpoint = calibrated_airspeed_sp, + .pitch_sp = NAN, + .thrust_sp = NAN + }; - const bool disable_underspeed_handling = false; + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - - tecs_update_pitch_throttle(control_interval, - _current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude. - calibrated_airspeed_sp, - min_pitch, - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - throttle_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - height_rate_sp); + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits.pitch_min = min_pitch; + longitudinal_control_limits.throttle_max = throttle_max; + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH || fabsf(_manual_control_setpoint.yaw) >= HDG_HOLD_MAN_INPUT_THRESH) { @@ -2344,16 +2108,21 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _hdg_hold_enabled = false; _yaw_lock_engaged = false; - roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = rollAngleToLateralAccel(roll_body); + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); } - _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); +} - pitch_body = get_tecs_pitch(); - - Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); +float FixedwingPositionControl::rollAngleToLateralAccel(float roll_body) const +{ + return tanf(roll_body) * CONSTANTS_ONE_G; } void FixedwingPositionControl::control_backtransition_heading_hold() @@ -2362,34 +2131,14 @@ void FixedwingPositionControl::control_backtransition_heading_hold() _backtrans_heading = _local_pos.heading; } - float true_airspeed = _airspeed_eas * _eas2tas; - - if (!_airspeed_valid) { - true_airspeed = _performance_model.getCalibratedTrimAirspeed() * _eas2tas; - } - - // we can achieve heading control by setting airspeed and groundspeed vector equal - const Vector2f airspeed_vector = Vector2f(cosf(_local_pos.heading), sinf(_local_pos.heading)) * true_airspeed; - const Vector2f &ground_speed = airspeed_vector; - - _npfg.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - - Vector2f virtual_target_point = Vector2f(cosf(_backtrans_heading), sinf(_backtrans_heading)) * HDG_HOLD_DIST_NEXT; - - navigateLine(Vector2f(0.f, 0.f), virtual_target_point, Vector2f(0.f, 0.f), ground_speed, Vector2f(0.f, 0.f)); - - const float roll_body = getCorrectedNpfgRollSetpoint(); - - const float yaw_body = _backtrans_heading; - - // these values are overriden by transition logic - _att_sp.thrust_body[0] = _param_fw_thr_min.get(); - const float pitch_body = 0.0f; - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.airspeed_reference_direction = _backtrans_heading; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); } void FixedwingPositionControl::control_backtransition_line_follow(const Vector2f &ground_speed, @@ -2398,8 +2147,6 @@ void FixedwingPositionControl::control_backtransition_line_follow(const Vector2f Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); - _npfg.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); // Set the position where the backtransition started the first ime we pass through here. // Will get reset if not in transition anymore. @@ -2407,38 +2154,21 @@ void FixedwingPositionControl::control_backtransition_line_follow(const Vector2f _lpos_where_backtrans_started = curr_pos_local; } - navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); - const float roll_body = getCorrectedNpfgRollSetpoint(); + DirectionalGuidanceOutput sp = {.lateral_acceleration_feedforward = 0.0f}; - const float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - // these values are overriden by transition logic - _att_sp.thrust_body[0] = _param_fw_thr_min.get(); - const float pitch_body = 0.0f; - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); -} -float -FixedwingPositionControl::get_tecs_pitch() -{ - if (_tecs_is_running) { - return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get()); + if (_control_mode.flag_control_position_enabled) { + sp = navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } - // return level flight pitch offset to prevent stale tecs state when it's not running - return radians(_param_fw_psp_off.get()); -} + fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); -float -FixedwingPositionControl::get_tecs_thrust() -{ - 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; + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); } void @@ -2488,11 +2218,6 @@ FixedwingPositionControl::Run() // handle estimator reset events. we only adjust setpoins for manual modes if (_control_mode.flag_control_manual_enabled) { - if (_control_mode.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(_current_altitude, -_local_pos.vz); - } - // adjust navigation waypoints in position control mode if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled && _local_pos.xy_reset_counter != _xy_reset_counter) { @@ -2606,8 +2331,6 @@ FixedwingPositionControl::Run() if (_vehicle_air_data_sub.update(&air_data)) { _air_density = PX4_ISFINITE(air_data.rho) ? air_data.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_land_detected_sub.updated()) { @@ -2634,10 +2357,9 @@ FixedwingPositionControl::Run() update_in_air_states(_local_pos.timestamp); // restore nominal TECS parameters in case changed intermittently (e.g. in landing handling) - _tecs.set_speed_weight(_param_fw_t_spdweight.get()); // restore lateral-directional guidance parameters (changed in takeoff mode) - _npfg.setPeriod(_param_npfg_period.get()); + _directional_guidance.setPeriod(_param_npfg_period.get()); _att_sp.reset_integral = false; @@ -2645,8 +2367,6 @@ FixedwingPositionControl::Run() _flaps_setpoint = 0.f; _spoilers_setpoint = 0.f; - // reset flight phase estimate - _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; // by default we don't want yaw to be contoller directly with rudder _att_sp.fw_control_yaw_wheel = false; @@ -2731,52 +2451,6 @@ FixedwingPositionControl::Run() } - if (_control_mode_current != FW_POSCTRL_MODE_OTHER) { - Eulerf attitude_setpoint(Quatf(_att_sp.q_d)); - float roll_body = attitude_setpoint.phi(); - float pitch_body = attitude_setpoint.theta(); - float yaw_body = attitude_setpoint.psi(); - - if (_control_mode.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.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_acceleration_enabled || - _control_mode.flag_control_altitude_enabled || - _control_mode.flag_control_climb_rate_enabled) { - - // roll slew rate - roll_body = _roll_slew_rate.update(roll_body, control_interval); - - const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); - q.copyTo(_att_sp.q_d); - - _att_sp.timestamp = hrt_absolute_time(); - _attitude_sp_pub.publish(_att_sp); - - // only publish status in full FW mode - if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - || _vehicle_status.in_transition_mode) { - status_publish(); - - } - } - - } else { - _roll_slew_rate.setForcedValue(_roll); - } - - - - // Publish estimate of level flight - _flight_phase_estimation_pub.get().timestamp = hrt_absolute_time(); - _flight_phase_estimation_pub.update(); - // if there's any change in landing gear setpoint publish it if (_new_landing_gear_position != old_landing_gear_position && _new_landing_gear_position != landing_gear_s::GEAR_KEEP) { @@ -2801,7 +2475,6 @@ FixedwingPositionControl::Run() _spoilers_setpoint_pub.publish(spoilers_setpoint); } - _z_reset_counter = _local_pos.z_reset_counter; _xy_reset_counter = _local_pos.xy_reset_counter; perf_end(_loop_perf); @@ -2838,80 +2511,7 @@ FixedwingPositionControl::reset_landing_state() } } -void -FixedwingPositionControl::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, const bool is_low_height, - 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.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 */ - 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(_pitch - radians(_param_fw_psp_off.get()), - _current_altitude, - alt_sp, - airspeed_sp, - _airspeed_eas, - _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, - -_local_pos.vz, - hgt_rate_sp); - - tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated); - - if (_tecs_is_running && !_vehicle_status.in_transition_mode - && (_vehicle_status.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(_current_altitude - 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 - _current_altitude) >= _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 (((_current_altitude - 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 - } - } -} - -float -FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, const float altitude, - const float terrain_altitude) const +float FixedwingPositionControl::getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const { // we want the wings level when at the wing height above ground const float height_above_ground = math::max(altitude - (terrain_altitude + _param_fw_wing_height.get()), 0.0f); @@ -2921,11 +2521,10 @@ FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, con // d(roll strike)/d(height) = 2 / span / cos(2 * height / span) // d(roll strike)/d(height) (@height=0) = 2 / span // roll strike ~= 2 * height / span - const float roll_wingtip_strike = 2.0f * height_above_ground / _param_fw_wing_span.get(); - - return math::constrain(roll_setpoint, -roll_wingtip_strike, roll_wingtip_strike); + return 2.0f * height_above_ground / _param_fw_wing_span.get(); } + void FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev, const float land_point_altitude, const Vector2f &local_position, const Vector2f &local_land_point) @@ -2985,27 +2584,6 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po } } -bool FixedwingPositionControl::checkLowHeightConditions() -{ - // 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 FixedwingPositionControl::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); - _tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState()); -} - Vector2f FixedwingPositionControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position) { @@ -3125,9 +2703,6 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo local_position_setpoint.acceleration[0] = NAN; local_position_setpoint.acceleration[1] = NAN; local_position_setpoint.acceleration[2] = NAN; - local_position_setpoint.thrust[0] = _att_sp.thrust_body[0]; - local_position_setpoint.thrust[1] = _att_sp.thrust_body[1]; - local_position_setpoint.thrust[2] = _att_sp.thrust_body[2]; _local_pos_sp_pub.publish(local_position_setpoint); } @@ -3150,7 +2725,8 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_ _orbit_status_pub.publish(orbit_status); } -void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint, const Vector2f &end_waypoint, +DirectionalGuidanceOutput FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint, + const Vector2f &end_waypoint, const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) { const Vector2f start_waypoint_to_end_waypoint = end_waypoint - start_waypoint; @@ -3160,15 +2736,13 @@ void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint, if (start_waypoint_to_end_waypoint.norm() < FLT_EPSILON) { // degenerate case: the waypoints are on top of each other, this should only happen when someone uses this // method incorrectly. just as a safe guard, call the singular waypoint navigation method. - navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); - return; + return navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); } if ((start_waypoint_to_end_waypoint.dot(start_waypoint_to_vehicle) < -FLT_EPSILON) - && (start_waypoint_to_vehicle.norm() > _npfg.switchDistance(500.0f))) { + && (start_waypoint_to_vehicle.norm() > _directional_guidance.switchDistance(500.0f))) { // we are in front of the start waypoint, fly directly to it until we are within switch distance - navigateWaypoint(start_waypoint, vehicle_pos, ground_vel, wind_vel); - return; + return navigateWaypoint(start_waypoint, vehicle_pos, ground_vel, wind_vel); } if (start_waypoint_to_end_waypoint.dot(end_waypoint_to_vehicle) > FLT_EPSILON) { @@ -3177,42 +2751,43 @@ void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint, // end waypoint. however this included here as a safety precaution if any navigator (module) switch condition // is missed for any reason. in the future this logic should all be handled in one place in a dedicated // flight mode state machine. - navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); - return; + return navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); } // follow the line segment between the start and end waypoints - navigateLine(start_waypoint, end_waypoint, vehicle_pos, ground_vel, wind_vel); + return navigateLine(start_waypoint, end_waypoint, vehicle_pos, ground_vel, wind_vel); } -void FixedwingPositionControl::navigateWaypoint(const Vector2f &waypoint_pos, const Vector2f &vehicle_pos, +DirectionalGuidanceOutput FixedwingPositionControl::navigateWaypoint(const Vector2f &waypoint_pos, + const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) { const Vector2f vehicle_to_waypoint = waypoint_pos - vehicle_pos; if (vehicle_to_waypoint.norm() < FLT_EPSILON) { // degenerate case: the vehicle is on top of the single waypoint. (can happen). maintain the last npfg command. - return; + return DirectionalGuidanceOutput{}; } const Vector2f unit_path_tangent = vehicle_to_waypoint.normalized(); _closest_point_on_path = waypoint_pos; const float path_curvature = 0.f; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, + _closest_point_on_path, path_curvature); - // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. - _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); + return sp; } -void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, +DirectionalGuidanceOutput FixedwingPositionControl::navigateLine(const Vector2f &point_on_line_1, + const Vector2f &point_on_line_2, const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) { const Vector2f line_segment = point_on_line_2 - point_on_line_1; if (line_segment.norm() <= FLT_EPSILON) { // degenerate case: line segment has zero length. maintain the last npfg command. - return; + return DirectionalGuidanceOutput{}; } const Vector2f unit_path_tangent = line_segment.normalized(); @@ -3221,13 +2796,15 @@ void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line_1, con _closest_point_on_path = point_on_line_1 + point_1_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; const float path_curvature = 0.f; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + const DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, + unit_path_tangent, + _closest_point_on_path, path_curvature); - // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. - _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); + return sp; } -void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line, const float line_bearing, +DirectionalGuidanceOutput FixedwingPositionControl::navigateLine(const Vector2f &point_on_line, + const float line_bearing, const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) { const Vector2f unit_path_tangent{cosf(line_bearing), sinf(line_bearing)}; @@ -3236,13 +2813,15 @@ void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line, const _closest_point_on_path = point_on_line + point_on_line_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; const float path_curvature = 0.f; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + const DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, + unit_path_tangent, + _closest_point_on_path, path_curvature); - // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. - _target_bearing = line_bearing; + return sp; } -void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos, +DirectionalGuidanceOutput FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, + const Vector2f &vehicle_pos, float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel) { const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f; @@ -3274,37 +2853,36 @@ void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, con // 90 deg clockwise rotation * loiter direction const Vector2f unit_path_tangent = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)}; - float path_curvature = loiter_direction_multiplier / radius; - _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); + const float path_curvature = loiter_direction_multiplier / radius; _closest_point_on_path = unit_vec_center_to_closest_pt * radius + loiter_center; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, - loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature); + return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, + loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature); } -void FixedwingPositionControl::navigatePathTangent(const matrix::Vector2f &vehicle_pos, +DirectionalGuidanceOutput FixedwingPositionControl::navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint, const matrix::Vector2f &tangent_setpoint, const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature) { if (tangent_setpoint.norm() <= FLT_EPSILON) { // degenerate case: no direction. maintain the last npfg command. - return; + return DirectionalGuidanceOutput{}; } const Vector2f unit_path_tangent{tangent_setpoint.normalized()}; - _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = position_setpoint; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, tangent_setpoint.normalized(), position_setpoint, curvature); + return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, tangent_setpoint.normalized(), + position_setpoint, + curvature); } -void FixedwingPositionControl::navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, +DirectionalGuidanceOutput FixedwingPositionControl::navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, const Vector2f &ground_vel, const Vector2f &wind_vel) { const Vector2f unit_path_tangent = Vector2f{cosf(bearing), sinf(bearing)}; - _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = vehicle_pos; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, vehicle_pos, 0.0f); + return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, vehicle_pos, 0.0f); } int FixedwingPositionControl::task_spawn(int argc, char *argv[]) @@ -3363,7 +2941,7 @@ fw_pos_control is the fixed-wing position controller. return 0; } -float FixedwingPositionControl::getLoadFactor() +float FixedwingPositionControl::getLoadFactor() const { float load_factor_from_bank_angle = 1.0f; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index e56263c5e3..4ad6ba12f8 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -56,8 +56,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -98,6 +97,10 @@ #include #include #include +#include +#include +#include +#include #ifdef CONFIG_FIGURE_OF_EIGHT #include "figure_eight/FigureEight.hpp" @@ -115,12 +118,6 @@ using matrix::Vector2f; // [m] initial distance of waypoint in front of plane in heading hold mode static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f; -// [m] distance (plane to waypoint in front) at which waypoints are reset in heading hold mode -static constexpr float HDG_HOLD_REACHED_DIST = 1000.0f; - -// [m] distance by which previous waypoint is set behind the plane -static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; - // [rad/s] max yawrate at which plane locks yaw for heading hold mode static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f; @@ -169,14 +166,7 @@ static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f; // [m/s] maximum reference altitude rate threshhold static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f; -// [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; - -// [s] slew rate with which we change altitude time constant -static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f; class FixedwingPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem @@ -217,18 +207,19 @@ private: uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Publication _attitude_sp_pub; uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; - uORB::Publication _npfg_status_pub{ORB_ID(npfg_status)}; uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; - uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; uORB::Publication _launch_detection_status_pub{ORB_ID(launch_detection_status)}; uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)}; uORB::Publication _landing_gear_pub {ORB_ID(landing_gear)}; uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; - uORB::PublicationData _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)}; + uORB::PublicationData _lateral_ctrl_sp_pub{ORB_ID(fw_lateral_control_setpoint)}; + uORB::PublicationData _lateral_ctrl_status_pub{ORB_ID(fw_lateral_control_status)}; + uORB::PublicationData _lateral_ctrl_limits_pub{ORB_ID(lateral_control_limits)}; + uORB::PublicationData _longitudinal_ctrl_sp_pub{ORB_ID(fw_longitudinal_control_setpoint)}; + uORB::PublicationData _longitudinal_ctrl_limits_pub{ORB_ID(longitudinal_control_limits)}; manual_control_setpoint_s _manual_control_setpoint{}; position_setpoint_triplet_s _pos_sp_triplet{}; @@ -276,8 +267,6 @@ private: double _current_longitude{0}; float _current_altitude{0.f}; - float _roll{0.f}; - float _pitch{0.0f}; float _yaw{0.0f}; float _yawrate{0.0f}; @@ -397,24 +386,12 @@ private: hrt_abstime _time_wind_last_received{0}; // [us] - // TECS - - // total energy control system - airspeed / altitude control - TECS _tecs; - - bool _tecs_is_running{false}; - - // Smooths changes in the altitude tracking error time constant value - SlewRate _tecs_alt_time_const_slew_rate; - // VTOL / TRANSITION - bool _is_vtol_tailsitter{false}; matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN}; float _backtrans_heading{NAN}; // used to lock the initial heading for backtransition with no position control // ESTIMATOR RESET COUNTERS uint8_t _xy_reset_counter{0}; - uint8_t _z_reset_counter{0}; uint64_t _time_last_xy_reset{0}; // LATERAL-DIRECTIONAL GUIDANCE @@ -423,10 +400,7 @@ private: matrix::Vector2f _closest_point_on_path; // nonlinear path following guidance - lateral-directional position control - NPFG _npfg; - 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 + DirectionalGuidance _directional_guidance; PerformanceModel _performance_model; @@ -439,7 +413,6 @@ private: hrt_abstime _time_in_fixed_bank_loiter{0}; // [us] float _min_current_sp_distance_xy{FLT_MAX}; - float _target_bearing{0.0f}; // [rad] #ifdef CONFIG_FIGURE_OF_EIGHT /* Loitering */ @@ -465,27 +438,18 @@ private: // Update subscriptions void airspeed_poll(); - void control_update(); + void manual_control_setpoint_poll(); void vehicle_attitude_poll(); void vehicle_command_poll(); void vehicle_control_mode_poll(); - void vehicle_status_poll(); + void wind_poll(); - void status_publish(); void landing_status_publish(); - void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw, - float throttle_trim); - void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint); - float getLoadFactor(); - /** - * @brief Get the NPFG roll setpoint with mitigation strategy if npfg is not certain about its output - * - * @return roll setpoint - */ - float getCorrectedNpfgRollSetpoint(); + void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint); + float getLoadFactor() const; /** * @brief Sets the landing abort status and publishes landing status. @@ -503,24 +467,6 @@ private: */ bool checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, uint8_t landing_abort_criterion); - /** - * @brief Get a new waypoint based on heading and distance from current position - * - * @param heading the heading to fly to - * @param distance the distance of the generated waypoint - * @param waypoint_prev the waypoint at the current position - * @param waypoint_next the waypoint in the heading direction - */ - void get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev, - position_setpoint_s &waypoint_next, bool flag_init); - - /** - * @brief Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available - * - * @param takeoff_alt Altitude AMSL at launch or when runway takeoff is detected [m] - */ - float get_terrain_altitude_takeoff(float takeoff_alt); - /** * @brief Maps the manual control setpoint (pilot sticks) to height rate commands * @@ -535,13 +481,6 @@ private: */ void updateManualTakeoffStatus(); - /** - * @brief Update desired altitude base on user pitch stick input - * - * @param dt Time step - */ - void update_desired_altitude(float dt); - /** * @brief Updates timing information for landed and in-air states. * @@ -724,9 +663,6 @@ private: void control_backtransition_line_follow(const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr); - float get_tecs_pitch(); - float get_tecs_thrust(); - float get_manual_airspeed_setpoint(); /** @@ -759,38 +695,8 @@ private: void publishOrbitStatus(const position_setpoint_s pos_sp); SlewRate _airspeed_slew_rate_controller; - SlewRate _roll_slew_rate; - /** - * @brief A wrapper function to call the TECS implementation - * - * @param control_interval Time since the last position control update [s] - * @param alt_sp Altitude setpoint, AMSL [m] - * @param airspeed_sp Calibrated airspeed setpoint [m/s] - * @param pitch_min_rad Nominal pitch angle command minimum [rad] - * @param pitch_max_rad Nominal pitch angle command maximum [rad] - * @param throttle_min Minimum throttle command [0,1] - * @param throttle_max Maximum throttle command [0,1] - * @param desired_max_sink_rate The desired max sink rate commandable when altitude errors are large [m/s] - * @param desired_max_climb_rate The desired max climb rate commandable when altitude errors are large [m/s] - * @param is_low_height Define whether we are in low-height flight for tighter altitude tracking - * @param disable_underspeed_detection True if underspeed detection should be disabled - * @param hgt_rate_sp Height rate setpoint [m/s] - */ - 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_sink_rate, const float desired_max_climb_rate, const bool is_low_height, - bool disable_underspeed_detection = false, float hgt_rate_sp = NAN); - - /** - * @brief Constrains the roll angle setpoint near ground to avoid wingtip strike. - * - * @param roll_setpoint Unconstrained roll angle setpoint [rad] - * @param altitude Vehicle altitude (AMSL) [m] - * @param terrain_altitude Terrain altitude (AMSL) [m] - * @return Constrained roll angle setpoint [rad] - */ - float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const; + float getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const; /** * @brief Calculates the touchdown position for landing with optional manual lateral adjustments. @@ -838,21 +744,6 @@ private: void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev, const float land_point_alt, const Vector2f &local_position, const Vector2f &local_land_point); - /* - * Checks if the vehicle satisfies conditions for low-height flight - * - * @return bool True if conditions are satisfied, false otherwise - */ - bool checkLowHeightConditions(); - - /* - * Updates TECS altitude time constant according to the is_low_height parameter. - * - * @param is_low_height Boolean flag defining whether we are in low-height flight - * @param dt Update time step [s] - */ - void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt); - /* * Waypoint handling logic following closely to the ECL_L1_Pos_Controller * method of the same name. Takes two waypoints, steering the vehicle to track @@ -864,9 +755,10 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateWaypoints(const matrix::Vector2f &start_waypoint, const matrix::Vector2f &end_waypoint, - const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel, - const matrix::Vector2f &wind_vel); + DirectionalGuidanceOutput navigateWaypoints(const matrix::Vector2f &start_waypoint, + const matrix::Vector2f &end_waypoint, + const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel, + const matrix::Vector2f &wind_vel); /* * Takes one waypoint and steers the vehicle towards this. @@ -879,8 +771,8 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos, - const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel); + DirectionalGuidanceOutput navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos, + const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel); /* * Line (infinite) following logic. Two points on the line are used to define the @@ -893,8 +785,9 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, const Vector2f &vehicle_pos, - const Vector2f &ground_vel, const Vector2f &wind_vel); + DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, + const Vector2f &vehicle_pos, + const Vector2f &ground_vel, const Vector2f &wind_vel); /* * Line (infinite) following logic. One point on the line and a line bearing are used to define @@ -907,8 +800,9 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateLine(const Vector2f &point_on_line, const float line_bearing, const Vector2f &vehicle_pos, - const Vector2f &ground_vel, const Vector2f &wind_vel); + DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line, const float line_bearing, + const Vector2f &vehicle_pos, + const Vector2f &ground_vel, const Vector2f &wind_vel); /* * Loitering (unlimited) logic. Takes loiter center, radius, and direction and @@ -922,9 +816,9 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos, - float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel, - const matrix::Vector2f &wind_vel); + DirectionalGuidanceOutput navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos, + float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel, + const matrix::Vector2f &wind_vel); /* * Path following logic. Takes poisiton, path tangent, curvature and @@ -939,9 +833,10 @@ private: * @param[in] wind_vel Wind velocity vector [m/s] * @param[in] curvature of the path setpoint [1/m] */ - void navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint, - const matrix::Vector2f &tangent_setpoint, - const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature); + DirectionalGuidanceOutput navigatePathTangent(const matrix::Vector2f &vehicle_pos, + const matrix::Vector2f &position_setpoint, + const matrix::Vector2f &tangent_setpoint, + const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature); /* * Navigate on a fixed bearing. @@ -954,10 +849,12 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, const matrix::Vector2f &ground_vel, - const matrix::Vector2f &wind_vel); + DirectionalGuidanceOutput navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, + const matrix::Vector2f &ground_vel, + const matrix::Vector2f &wind_vel); DEFINE_PARAMETERS( + (ParamFloat) _param_fw_psp_off, (ParamFloat) _param_fw_gnd_spd_min, (ParamFloat) _param_fw_pn_r_slew_max, @@ -980,7 +877,6 @@ private: (ParamFloat) _param_fw_lnd_fl_pmax, (ParamFloat) _param_fw_lnd_fl_pmin, (ParamFloat) _param_fw_lnd_flalt, - (ParamFloat) _param_fw_thrtc_sc, (ParamFloat) _param_fw_t_thr_low_hgt, (ParamBool) _param_fw_lnd_earlycfg, (ParamInt) _param_fw_lnd_useter, @@ -988,30 +884,14 @@ private: (ParamFloat) _param_fw_p_lim_max, (ParamFloat) _param_fw_p_lim_min, - (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_spdweight, - (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_climbrate_target, (ParamFloat) _param_sinkrate_target, - (ParamFloat) _param_speed_standard_dev, - (ParamFloat) _param_speed_rate_standard_dev, - (ParamFloat) _param_process_noise_standard_dev, + (ParamFloat) _param_fw_t_sink_max, + (ParamFloat) _param_fw_t_spdweight, (ParamFloat) _param_fw_thr_idle, (ParamFloat) _param_fw_thr_max, (ParamFloat) _param_fw_thr_min, - (ParamFloat) _param_fw_thr_slew_max, (ParamFloat) _param_fw_flaps_lnd_scl, (ParamFloat) _param_fw_flaps_to_scl, @@ -1025,8 +905,6 @@ private: // external parameters (ParamBool) _param_fw_use_airspd, - (ParamFloat) _param_fw_psp_off, - (ParamFloat) _param_nav_loiter_rad, (ParamFloat) _takeoff_pitch_min, @@ -1054,6 +932,11 @@ private: (ParamBool) _param_fw_laun_detcn_on ) + void control_idle(); + + void setDefaultLongControlLimits(longitudinal_control_limits_s &longitudinal_control_limits) const; + + float rollAngleToLateralAccel(float roll_body) const; }; #endif // FIXEDWINGPOSITIONCONTROL_HPP_ diff --git a/src/modules/fw_pos_control/figure_eight/FigureEight.cpp b/src/modules/fw_pos_control/figure_eight/FigureEight.cpp index d0ce50a033..91c02b7bce 100644 --- a/src/modules/fw_pos_control/figure_eight/FigureEight.cpp +++ b/src/modules/fw_pos_control/figure_eight/FigureEight.cpp @@ -48,11 +48,10 @@ static constexpr bool SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE{true}; static constexpr float DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO{2.5f}; static constexpr float MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO{2.0f}; -FigureEight::FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas) : +FigureEight::FigureEight(DirectionalGuidance &npfg, matrix::Vector2f &wind_vel, float &eas2tas) : ModuleParams(nullptr), - _npfg(npfg), - _wind_vel(wind_vel), - _eas2tas(eas2tas) + _directional_guidance(npfg), + _wind_vel(wind_vel) { } @@ -64,8 +63,9 @@ void FigureEight::resetPattern() _pos_passed_circle_center_along_major_axis = false; } -void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed) +DirectionalGuidanceOutput FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, + const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, float target_airspeed) { // Sanitize inputs FigureEightPatternParameters valid_parameters{sanitizeParameters(parameters)}; @@ -81,7 +81,7 @@ void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const m updateSegment(curr_pos_local, valid_parameters, pattern_points); // Apply control logic based on segment - applyControl(curr_pos_local, ground_speed, valid_parameters, target_airspeed, pattern_points); + return applyControl(curr_pos_local, ground_speed, valid_parameters, target_airspeed, pattern_points); } FigureEight::FigureEightPatternParameters FigureEight::sanitizeParameters(const FigureEightPatternParameters @@ -118,7 +118,8 @@ void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, cons const bool north_is_closer = north_center_to_pos_local.norm() < south_center_to_pos_local.norm(); // Get the normalized switch distance. - float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius; + float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / + parameters.loiter_radius; //Far away from current figure of eight. Fly towards closer circle @@ -194,7 +195,8 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters); // Get the normalized switch distance. - float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius; + float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / + parameters.loiter_radius; // Update segment if segment exit condition has been reached switch (_current_segment) { @@ -275,56 +277,60 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi } } -void FigureEight::applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed, - const FigureEightPatternPoints &pattern_points) +DirectionalGuidanceOutput FigureEight::applyControl(const matrix::Vector2f &curr_pos_local, + const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, float target_airspeed, + const FigureEightPatternPoints &pattern_points) { Vector2f center_to_pos_local; calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters); switch (_current_segment) { case FigureEightSegment::SEGMENT_CIRCLE_NORTH: { - applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local, + ground_speed, parameters, target_airspeed); } break; case FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST: { // Follow path from north-east to south-west - applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset, + curr_pos_local, + ground_speed, parameters, target_airspeed); } break; case FigureEightSegment::SEGMENT_CIRCLE_SOUTH: { - applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local, + ground_speed, parameters, target_airspeed); } break; case FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST: { // follow path from south-east to north-west - applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset, + curr_pos_local, + ground_speed, parameters, target_airspeed); } break; case FigureEightSegment::SEGMENT_POINT_SOUTHWEST: { // Follow path from current position to south-west - applyLine(center_to_pos_local, pattern_points.normalized_south_entry_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyLine(center_to_pos_local, pattern_points.normalized_south_entry_offset, curr_pos_local, + ground_speed, parameters, target_airspeed); } break; case FigureEightSegment::SEGMENT_POINT_NORTHWEST: { // Follow path from current position to north-west - applyLine(center_to_pos_local, pattern_points.normalized_north_entry_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyLine(center_to_pos_local, pattern_points.normalized_north_entry_offset, curr_pos_local, + ground_speed, parameters, target_airspeed); } break; case FigureEightSegment::SEGMENT_UNDEFINED: default: + return DirectionalGuidanceOutput{}; break; } } @@ -356,9 +362,10 @@ float FigureEight::calculateRotationAngle(const FigureEightPatternParameters &pa return yaw_rotation; } -void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset, - const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed) +DirectionalGuidanceOutput FigureEight::applyCircle(bool loiter_direction_counter_clockwise, + const matrix::Vector2f &normalized_circle_offset, + const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, float target_airspeed) { const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f; @@ -366,8 +373,6 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat Vector2f circle_offset_rotated = Dcm2f(calculateRotationAngle(parameters)) * circle_offset; Vector2f circle_center = parameters.center_pos_local + circle_offset_rotated; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); const Vector2f vector_center_to_vehicle = curr_pos_local - circle_center; const float dist_to_center = vector_center_to_vehicle.norm(); @@ -392,16 +397,14 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat float path_curvature = loiter_direction_multiplier / parameters.loiter_minor_radius; _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = unit_vec_center_to_closest_pt * parameters.loiter_minor_radius + circle_center; - _npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent, - _closest_point_on_path, path_curvature); + return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent, + _closest_point_on_path, path_curvature); - _roll_setpoint = _npfg.getRollSetpoint(); - _indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas; } -void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset, - const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local, - const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters, float target_airspeed) +DirectionalGuidanceOutput FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset, + const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local, + const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters, float target_airspeed) { const Dcm2f rotation_matrix(calculateRotationAngle(parameters)); @@ -414,15 +417,12 @@ void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset const Vector2f end_offset_rotated = rotation_matrix * end_offset; const Vector2f line_segment_end_position = parameters.center_pos_local + end_offset_rotated; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); const Vector2f path_tangent = line_segment_end_position - line_segment_start_position; const Vector2f unit_path_tangent = path_tangent.normalized(); _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); const Vector2f vector_A_to_vehicle = curr_pos_local - line_segment_start_position; _closest_point_on_path = line_segment_start_position + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; - _npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), line_segment_start_position, - 0.0f); - _roll_setpoint = _npfg.getRollSetpoint(); - _indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas; + return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), + line_segment_start_position, + 0.0f); } diff --git a/src/modules/fw_pos_control/figure_eight/FigureEight.hpp b/src/modules/fw_pos_control/figure_eight/FigureEight.hpp index b3c55c840b..5dabbfb504 100644 --- a/src/modules/fw_pos_control/figure_eight/FigureEight.hpp +++ b/src/modules/fw_pos_control/figure_eight/FigureEight.hpp @@ -45,7 +45,7 @@ #include #include -#include "lib/npfg/npfg.hpp" +#include "lib/npfg/DirectionalGuidance.hpp" class FigureEight : public ModuleParams { @@ -89,7 +89,7 @@ public: * @param[in] wind_vel is the reference to the parent wind velocity [m/s]. * @param[in] eas2tas is the reference to the parent indicated airspeed to true airspeed conversion. */ - FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas); + FigureEight(DirectionalGuidance &directional_guidance, matrix::Vector2f &wind_vel, float &eas2tas); /** * @brief reset the figure eight pattern. @@ -107,20 +107,8 @@ public: * @param[in] parameters is the parameter set defining the figure eight shape. * @param[in] target_airspeed is the current targeted indicated airspeed [m/s]. */ - void updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed); - /** - * @brief Get the roll setpoint - * - * @return the roll setpoint in [rad]. - */ - float getRollSetpoint() const {return _roll_setpoint;}; - /** - * @brief Get the indicated airspeed setpoint - * - * @return the indicated airspeed setpoint in [m/s]. - */ - float getAirspeedSetpoint() const {return _indicated_airspeed_setpoint;}; + DirectionalGuidanceOutput updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, float target_airspeed); /** * @brief Get the target bearing of current point on figure of eight * @@ -175,9 +163,9 @@ private: * @param[in] target_airspeed is the current targeted indicated airspeed [m/s]. * @param[in] pattern_points are the relevant points defining the figure eight pattern. */ - void applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed, - const FigureEightPatternPoints &pattern_points); + DirectionalGuidanceOutput applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, float target_airspeed, + const FigureEightPatternPoints &pattern_points); /** * @brief Update active segment. * @@ -214,9 +202,10 @@ private: * @param[in] parameters is the parameter set defining the figure eight shape. * @param[in] target_airspeed is the current targeted indicated airspeed [m/s]. */ - void applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset, - const matrix::Vector2f &curr_pos_local, - const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters, float target_airspeed); + DirectionalGuidanceOutput applyCircle(bool loiter_direction_counter_clockwise, + const matrix::Vector2f &normalized_circle_offset, + const matrix::Vector2f &curr_pos_local, + const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters, float target_airspeed); /** * @brief Apply path lateral control * @@ -227,16 +216,17 @@ private: * @param[in] parameters is the parameter set defining the figure eight shape. * @param[in] target_airspeed is the current targeted indicated airspeed [m/s]. */ - void applyLine(const matrix::Vector2f &normalized_line_start_offset, const matrix::Vector2f &normalized_line_end_offset, - const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed); + DirectionalGuidanceOutput applyLine(const matrix::Vector2f &normalized_line_start_offset, + const matrix::Vector2f &normalized_line_end_offset, + const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, float target_airspeed); private: /** * @brief npfg lateral control object. * */ - NPFG &_npfg; + DirectionalGuidance &_directional_guidance; /** * @brief Wind velocity in [m/s]. @@ -244,24 +234,9 @@ private: */ const matrix::Vector2f &_wind_vel; /** - * @brief Conversion factor from indicated to true airspeed. - * - */ - const float &_eas2tas; - /** - * @brief Roll setpoint in [rad]. - * - */ - float _roll_setpoint; - /** - * @brief Indicated airspeed setpoint in [m/s]. - * - */ - float _indicated_airspeed_setpoint; - /** - * @brief active figure eight position setpoint. - * - */ + * @brief active figure eight position setpoint. + * + */ FigureEightPatternParameters _active_parameters; /** diff --git a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp index 7cfc23b438..805659c847 100644 --- a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp @@ -105,23 +105,23 @@ bool RunwayTakeoff::controlYaw() return takeoff_state_ < RunwayTakeoffState::CLIMBOUT; } -float RunwayTakeoff::getPitch(float external_pitch_setpoint) +float RunwayTakeoff::getPitch() { if (takeoff_state_ <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { return math::radians(param_rwto_psp_.get()); } - return external_pitch_setpoint; + return NAN; } -float RunwayTakeoff::getRoll(float external_roll_setpoint) +float RunwayTakeoff::getRoll() { // until we have enough ground clearance, set roll to 0 if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) { return 0.0f; } - return external_roll_setpoint; + return NAN; } float RunwayTakeoff::getYaw(float external_yaw_setpoint) @@ -134,7 +134,7 @@ float RunwayTakeoff::getYaw(float external_yaw_setpoint) } } -float RunwayTakeoff::getThrottle(const float idle_throttle, const float external_throttle_setpoint) const +float RunwayTakeoff::getThrottle(const float idle_throttle) const { float throttle = idle_throttle; @@ -147,19 +147,13 @@ float RunwayTakeoff::getThrottle(const float idle_throttle, const float external break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + case RunwayTakeoffState::CLIMBOUT: throttle = param_rwto_max_thr_.get(); break; - case RunwayTakeoffState::CLIMBOUT: - // ramp in throttle setpoint over takeoff rotation transition time - throttle = interpolateValuesOverAbsoluteTime(param_rwto_max_thr_.get(), external_throttle_setpoint, takeoff_time_, - param_rwto_rot_time_.get()); - - break; - case RunwayTakeoffState::FLY: - throttle = external_throttle_setpoint; + throttle = NAN; } return throttle; diff --git a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h index 35e463077e..1907867b6f 100644 --- a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h @@ -116,13 +116,13 @@ public: * @param external_pitch_setpoint Externally commanded pitch angle setpoint (usually from TECS) [rad] * @return Pitch angle setpoint (limited while plane is on runway) [rad] */ - float getPitch(float external_pitch_setpoint); + float getPitch(); /** * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from path navigation) [rad] * @return Roll angle setpoint [rad] */ - float getRoll(float external_roll_setpoint); + float getRoll(); /** * @brief Returns the appropriate yaw angle setpoint. @@ -145,7 +145,7 @@ public: * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1] * @return Throttle setpoint, normalized [0,1] */ - float getThrottle(const float idle_throttle, const float external_throttle_setpoint) const; + float getThrottle(const float idle_throttle) const; /** * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad]