diff --git a/msg/NpfgStatus.msg b/msg/NpfgStatus.msg index ef5538b551..132c1f7f3f 100644 --- a/msg/NpfgStatus.msg +++ b/msg/NpfgStatus.msg @@ -14,3 +14,4 @@ float32 min_ground_speed_ref # minimum forward ground speed reference [m/s] float32 adapted_period # adapted period (if auto-tuning enabled) [s] float32 p_gain # controller proportional gain [rad/s] float32 time_const # controller time constant [s] +float32 can_run_factor # estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1] diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index 196e0b2699..a3d9b703f6 100644 --- a/src/lib/npfg/npfg.cpp +++ b/src/lib/npfg/npfg.cpp @@ -47,6 +47,32 @@ using matrix::Vector2d; using matrix::Vector2f; +float NPFG::canRun(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const +{ + if (is_wind_valid) { + // If we have a valid wind estimate, npfg is able to handle all degenerated cases + return 1.f; + } + + // NPFG can run without wind information as long as the system is not flying backwards and has a minimal ground speed + // Check the minimal ground speed. if it is greater than twice the standard deviation, we assume that we can infer a valid track angle + const Vector2f ground_vel(local_pos.vx, local_pos.vy); + const float ground_speed(ground_vel.norm()); + const float low_ground_speed_factor(math::constrain((ground_speed - HORIZONTAL_EVH_FACTOR_COURSE_INVALID * + local_pos.evh) / ((HORIZONTAL_EVH_FACTOR_COURSE_VALID - HORIZONTAL_EVH_FACTOR_COURSE_INVALID)*local_pos.evh), + 0.f, 1.f)); + + // Check that the angle between heading and track is not off too much. if it is greater than 90° we will be pushed back from the wind and the npfg will propably give a roll command in the wrong direction. + const Vector2f heading_vector(matrix::Dcm2f(local_pos.heading)*Vector2f({1.f, 0.f})); + const Vector2f ground_vel_norm(ground_vel.normalized()); + const float flying_forward_factor(math::constrain((heading_vector.dot(ground_vel_norm) - + COS_HEADING_TRACK_ANGLE_PUSHED_BACK) / ((COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK - + COS_HEADING_TRACK_ANGLE_PUSHED_BACK)), + 0.f, 1.f)); + + return flying_forward_factor * low_ground_speed_factor; +} + void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &ground_vel, const Vector2f &wind_vel, const Vector2f &unit_path_tangent, const Vector2f &position_on_path, const float path_curvature) diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/npfg.hpp index f9c6bf2c42..1d185d48ed 100644 --- a/src/lib/npfg/npfg.hpp +++ b/src/lib/npfg/npfg.hpp @@ -63,6 +63,8 @@ #include #include +#include + /* * NPFG * Lateral-directional nonlinear path following guidance logic with excess wind handling @@ -71,6 +73,17 @@ class NPFG { public: + /** + * @brief Can run + * + * Evaluation if all the necessary information are available such that npfg can produce meaningful results. + * + * @param[in] local_pos is the current vehicle local position uorb message + * @param[in] is_wind_valid flag if the wind estimation is valid + * @return estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1]. Can be used to define proper mitigation actions. + */ + + float canRun(const vehicle_local_position_s &local_pos, bool is_wind_valid) const; /* * Computes the lateral acceleration and airspeed references necessary to track * a path in wind (including excess wind conditions). @@ -270,6 +283,10 @@ public: float getRollSetpoint() { return roll_setpoint_; } private: + static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_VALID{3.f}; ///< Factor of velocity standard deviation above which course calculation is considered good enough + static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_INVALID{2.f}; ///< Factor of velocity standard deviation below which course calculation is considered unsafe + static constexpr float COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK{0.09f}; ///< Cos of Heading to track angle below which it is assumed that the vehicle is not pushed back by the wind ~cos(85°) + static constexpr float COS_HEADING_TRACK_ANGLE_PUSHED_BACK{0.f}; ///< Cos of Heading to track angle above which it is assumed that the vehicle is pushed back by the wind static constexpr float NPFG_EPSILON = 1.0e-6; // small number *bigger than machine epsilon static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m] diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 46a0c5e27a..9739fb7604 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -475,6 +475,7 @@ FixedwingPositionControl::status_publish() 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); @@ -504,6 +505,25 @@ 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(_npfg.canRun(_local_pos, _wind_valid)); + + if ((1.f - can_run_factor) < FLT_EPSILON) { + _need_report_npfg_uncertain_condition = true; + } + + if (((1.f - can_run_factor) > FLT_EPSILON) && _need_report_npfg_uncertain_condition) { + _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!"); + } + + return can_run_factor * (new_roll_setpoint); +} + void FixedwingPositionControl::updateLandingAbortStatus(const uint8_t new_abort_status) { @@ -1067,7 +1087,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } - _att_sp.roll_body = _npfg.getRollSetpoint(); + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw @@ -1116,7 +1136,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel); - _att_sp.roll_body = _npfg.getRollSetpoint(); + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.yaw_body = _yaw; @@ -1211,7 +1231,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); - _att_sp.roll_body = _npfg.getRollSetpoint(); + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw @@ -1397,7 +1417,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - _att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint()); + _att_sp.roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1495,7 +1515,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - _att_sp.roll_body = _npfg.getRollSetpoint(); + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1644,7 +1664,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = _npfg.getRollSetpoint(); + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); // use npfg's bearing to commanded course, controlled via yaw angle while on runway _att_sp.yaw_body = _npfg.getBearing(); @@ -1723,7 +1743,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = _npfg.getRollSetpoint(); + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); /* longitudinal guidance */ @@ -1849,7 +1869,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = _npfg.getRollSetpoint(); + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw @@ -1926,7 +1946,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = _npfg.getRollSetpoint(); + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); /* longitudinal guidance */ @@ -2075,7 +2095,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); - _att_sp.roll_body = _npfg.getRollSetpoint(); + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw @@ -2444,6 +2464,7 @@ FixedwingPositionControl::Run() } } + } // if there's any change in landing gear setpoint publish it diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index dc252fea30..c2cb806b6d 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -415,6 +415,7 @@ private: // 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 PerformanceModel _performance_model; @@ -468,6 +469,13 @@ private: 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(); + /** * @brief Sets the landing abort status and publishes landing status. *