diff --git a/src/lib/rtl/rtl_time_estimator.cpp b/src/lib/rtl/rtl_time_estimator.cpp index 055b62ccabca..101769844fec 100644 --- a/src/lib/rtl/rtl_time_estimator.cpp +++ b/src/lib/rtl/rtl_time_estimator.cpp @@ -51,7 +51,6 @@ RtlTimeEstimator::RtlTimeEstimator() : ModuleParams(nullptr) { _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); - _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); @@ -79,7 +78,6 @@ rtl_time_estimate_s RtlTimeEstimator::getEstimate() const void RtlTimeEstimator::update() { - _vehicle_status_sub.update(); _wind_sub.update(); if (_parameter_update_sub.updated()) { @@ -130,22 +128,11 @@ void RtlTimeEstimator::addWait(float time_s) } } -void RtlTimeEstimator::addDescendMCLand(float alt) -{ - if (PX4_ISFINITE(alt)) { - _is_valid = true; - - if (alt < -FLT_EPSILON && PX4_ISFINITE(alt)) { - _time_estimate += -alt / getHoverLandSpeed(); - } - } -} - float RtlTimeEstimator::getCruiseGroundSpeed(const matrix::Vector2f &direction_norm) { float cruise_speed = getCruiseSpeed(); - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { matrix::Vector2f wind = get_wind(); const float wind_along_dir = wind.dot(direction_norm); @@ -186,17 +173,17 @@ float RtlTimeEstimator::getCruiseSpeed() { float ret = 1e6f; - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) { ret = 1e6f; } - } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + } else if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) { ret = 1e6f; } - } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { + } else if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) { ret = 1e6f; } @@ -205,17 +192,6 @@ float RtlTimeEstimator::getCruiseSpeed() return ret; } -float RtlTimeEstimator::getHoverLandSpeed() -{ - float ret = 1e6f; - - if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) { - ret = 1e6f; - } - - return ret; -} - matrix::Vector2f RtlTimeEstimator::get_wind() { _wind_sub.update(); @@ -233,12 +209,12 @@ float RtlTimeEstimator::getClimbRate() { float ret = 1e6f; - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) { ret = 1e6f; } - } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + } else if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) { ret = 1e6f; @@ -252,12 +228,12 @@ float RtlTimeEstimator::getDescendRate() { float ret = 1e6f; - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) { ret = 1e6f; } - } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + } else if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) { ret = 1e6f; } diff --git a/src/lib/rtl/rtl_time_estimator.h b/src/lib/rtl/rtl_time_estimator.h index db0f8c234ea3..8c8519cd9b6a 100644 --- a/src/lib/rtl/rtl_time_estimator.h +++ b/src/lib/rtl/rtl_time_estimator.h @@ -63,12 +63,12 @@ class RtlTimeEstimator : public ModuleParams ~RtlTimeEstimator() = default; void update(); - void reset() { _time_estimate = 0.f; _is_valid = false;}; + void reset() { _time_estimate = 0.f; _is_valid = false; }; rtl_time_estimate_s getEstimate() const; void addDistance(float hor_dist, const matrix::Vector2f &hor_direction, float vert_dist); void addVertDistance(float alt); void addWait(float time_s); - void addDescendMCLand(float alt); + void setVehicleType(uint8_t vehicle_type) { _vehicle_type = vehicle_type; }; private: /** @@ -106,13 +106,6 @@ class RtlTimeEstimator : public ModuleParams */ float getCruiseSpeed(); - /** - * @brief Get the Hover Land Speed - * - * @return Hover land speed [m/s] - */ - float getHoverLandSpeed(); - /** * @brief Get the horizontal wind velocity * @@ -120,9 +113,11 @@ class RtlTimeEstimator : public ModuleParams */ matrix::Vector2f get_wind(); - float _time_estimate; /**< Accumulated time estimate [s] */ + float _time_estimate{0.f}; /**< Accumulated time estimate [s] */ bool _is_valid{false}; /**< Checks if time estimate is valid */ + uint8_t _vehicle_type{vehicle_status_s::VEHICLE_TYPE_UNKNOWN}; /**< the defined vehicle type to use for the calculation*/ + DEFINE_PARAMETERS( (ParamFloat) _param_rtl_time_factor, /**< Safety factory for safe time estimate */ (ParamInt) _param_rtl_time_margin /**< Safety margin for safe time estimate */ @@ -130,7 +125,6 @@ class RtlTimeEstimator : public ModuleParams param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; /**< MC climb velocity parameter */ param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; /**< MC descend velocity parameter */ - param_t _param_mpc_land_speed{PARAM_INVALID}; /**< MC land descend speed parameter */ param_t _param_fw_climb_rate{PARAM_INVALID}; /**< FW climb speed parameter */ param_t _param_fw_sink_rate{PARAM_INVALID}; /**< FW descend speed parameter */ @@ -139,7 +133,6 @@ class RtlTimeEstimator : public ModuleParams param_t _param_rover_cruise_speed{PARAM_INVALID}; /**< Rover cruise speed parameter */ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< Parameter update topic */ - uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; /**< wind topic */ }; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index cb33f350b7b1..e34f242d4ada 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -59,6 +59,7 @@ RTL::RTL(Navigator *navigator) : ModuleParams(navigator), _rtl_direct(navigator) { + _rtl_direct.initialize(); } void RTL::updateDatamanCache() @@ -157,24 +158,6 @@ void RTL::updateDatamanCache() _dataman_cache_landItem.update(); } -void RTL::on_inactivation() -{ - switch (_rtl_type) { - case RtlType::RTL_MISSION_FAST: // Fall through - case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through - case RtlType::RTL_DIRECT_MISSION_LAND: - _rtl_mission_type_handle->on_inactivation(); - break; - - case RtlType::RTL_DIRECT: - _rtl_direct.on_inactivation(); - break; - - default: - break; - } -} - void RTL::on_inactive() { _global_pos_sub.update(); @@ -187,21 +170,12 @@ void RTL::on_inactive() parameters_update(); - switch (_rtl_type) { - case RtlType::RTL_MISSION_FAST: - case RtlType::RTL_MISSION_FAST_REVERSE: - case RtlType::RTL_DIRECT_MISSION_LAND: - _rtl_mission_type_handle->on_inactive(); - break; - - case RtlType::RTL_DIRECT: - _rtl_direct.on_inactive(); - break; - - default: - break; + if (_rtl_mission_type_handle) { + _rtl_mission_type_handle->run(false); } + _rtl_direct.run(false); + // Limit inactive calculation to 0.5Hz hrt_abstime now{hrt_absolute_time()}; @@ -230,7 +204,10 @@ void RTL::publishRemainingTimeEstimate() case RtlType::RTL_DIRECT_MISSION_LAND: case RtlType::RTL_MISSION_FAST: case RtlType::RTL_MISSION_FAST_REVERSE: - estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate(); + if (_rtl_mission_type_handle) { + estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate(); + } + break; default: @@ -250,12 +227,10 @@ void RTL::on_activation() case RtlType::RTL_MISSION_FAST: // Fall through case RtlType::RTL_MISSION_FAST_REVERSE: _rtl_mission_type_handle->setReturnAltMin(_enforce_rtl_alt); - _rtl_mission_type_handle->on_activation(); break; case RtlType::RTL_DIRECT: _rtl_direct.setReturnAltMin(_enforce_rtl_alt); - _rtl_direct.on_activation(); break; default: @@ -279,16 +254,23 @@ void RTL::on_active() updateDatamanCache(); switch (_rtl_type) { - case RtlType::RTL_MISSION_FAST: - case RtlType::RTL_MISSION_FAST_REVERSE: + case RtlType::RTL_MISSION_FAST: // Fall through + case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through case RtlType::RTL_DIRECT_MISSION_LAND: - _rtl_mission_type_handle->on_active(); - _rtl_mission_type_handle->updateFailsafeChecks(); + if (_rtl_mission_type_handle) { + _rtl_mission_type_handle->run(true); + } + + _rtl_direct.run(false); break; case RtlType::RTL_DIRECT: - _rtl_direct.on_active(); - _rtl_direct.updateFailsafeChecks(); + _rtl_direct.run(true); + + if (_rtl_mission_type_handle) { + _rtl_mission_type_handle->run(false); + } + break; default: @@ -312,7 +294,10 @@ bool RTL::isLanding() case RtlType::RTL_MISSION_FAST: case RtlType::RTL_MISSION_FAST_REVERSE: case RtlType::RTL_DIRECT_MISSION_LAND: - is_landing = _rtl_mission_type_handle->isLanding(); + if (_rtl_mission_type_handle) { + is_landing = _rtl_mission_type_handle->isLanding(); + } + break; case RtlType::RTL_DIRECT: @@ -622,6 +607,10 @@ void RTL::init_rtl_mission_type() default: break; } + + if (_rtl_mission_type_handle) { + _rtl_mission_type_handle->initialize(); + } } void RTL::parameters_update() diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 4ef13cb7874d..18a7ad3ea194 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -78,7 +78,6 @@ class RTL : public NavigatorMode, public ModuleParams RTL_MISSION_FAST_REVERSE, }; - void on_inactivation() override; void on_inactive() override; void on_activation() override; void on_active() override; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index f255a9f11d47..2c4c172f8b4d 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -73,7 +73,6 @@ void RtlDirect::on_inactivation() void RtlDirect::on_activation() { _global_pos_sub.update(); - _land_detected_sub.update(); _vehicle_status_sub.update(); parameters_update(); @@ -120,6 +119,12 @@ void RtlDirect::on_active() } } +void RtlDirect::on_inactive() +{ + _global_pos_sub.update(); + _vehicle_status_sub.update(); +} + void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s loiter_pos) { _home_pos_sub.update(); @@ -421,7 +426,7 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() { _global_pos_sub.update(); _rtl_time_estimator.update(); - + _rtl_time_estimator.setVehicleType(_vehicle_status_sub.get().vehicle_type); _rtl_time_estimator.reset(); RTLState start_state_for_estimate; @@ -455,8 +460,13 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() matrix::Vector2f direction{}; get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon, &direction(0), &direction(1)); - _rtl_time_estimator.addDistance(get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, - land_approach.lat, land_approach.lon), direction, 0.f); + float move_to_land_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon)}; + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + move_to_land_dist = max(0.f, move_to_land_dist - land_approach.loiter_radius_m); + } + + _rtl_time_estimator.addDistance(move_to_land_dist, direction, 0.f); } // FALLTHROUGH @@ -527,7 +537,11 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() initial_altitude = loiter_altitude; } - _rtl_time_estimator.addDescendMCLand(_destination.alt - initial_altitude); + if (_vehicle_status_sub.get().is_vtol) { + _rtl_time_estimator.setVehicleType(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + } + + _rtl_time_estimator.addVertDistance(_destination.alt - initial_altitude); } break; @@ -560,7 +574,6 @@ loiter_point_s RtlDirect::sanitizeLandApproach(loiter_point_s land_approach) con if (!PX4_ISFINITE(land_approach.lat) || !PX4_ISFINITE(land_approach.lon)) { sanitized_land_approach.lat = _destination.lat; sanitized_land_approach.lon = _destination.lon; - } if (!PX4_ISFINITE(land_approach.height_m)) { diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index aed7cf3e4c6d..0a584234ecd2 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -91,6 +91,13 @@ class RtlDirect : public MissionBlock, public ModuleParams */ void on_active() override; + /** + * @brief on inactive + * Poll required topics also when incative for rtl time estimate. + * + */ + void on_inactive() override; + /** * @brief Calculate the estimated time needed to return to launch. * diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index bc6df21a49c3..d5256535c500 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -239,6 +239,7 @@ void RtlDirectMissionLand::setActiveMissionItems() rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() { _rtl_time_estimator.update(); + _rtl_time_estimator.setVehicleType(_vehicle_status_sub.get().vehicle_type); _rtl_time_estimator.reset(); if (_mission.count > 0 && hasMissionLandStart()) { @@ -247,7 +248,7 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() if (isActive()) { start_item_index = math::max(_mission.current_seq, _mission.land_start_index); - is_in_climbing_submode = _needs_climbing; + is_in_climbing_submode = _work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB; } else { start_item_index = _mission.land_start_index; @@ -305,7 +306,12 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + hor_position_at_calculation_point(1), next_position_mission_item.lat, + next_position_mission_item.lon); + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + hor_dist = math::max(0.f, hor_dist - next_position_mission_item.loiter_radius); + } _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); @@ -321,7 +327,12 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + hor_position_at_calculation_point(1), next_position_mission_item.lat, + next_position_mission_item.lon); + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + hor_dist = math::max(0.f, hor_dist - next_position_mission_item.loiter_radius); + } _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); @@ -339,14 +350,13 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); - float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + const float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), + hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); // For fixed wing, add diagonal line if ((_vehicle_status_sub.get().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && (!_vehicle_status_sub.get().is_vtol)) { - _rtl_time_estimator.addDistance(hor_dist, direction, get_absolute_altitude_for_item(next_position_mission_item) - altitude_at_calculation_point); @@ -354,8 +364,12 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() // For VTOL, Rotary, go there horizontally first, then land _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); - _rtl_time_estimator.addDescendMCLand(get_absolute_altitude_for_item(next_position_mission_item) - - altitude_at_calculation_point); + if (_vehicle_status_sub.get().is_vtol) { + _rtl_time_estimator.setVehicleType(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + } + + _rtl_time_estimator.addVertDistance(get_absolute_altitude_for_item(next_position_mission_item) - + altitude_at_calculation_point); } break; @@ -367,8 +381,8 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); - float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + const float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), + hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); _rtl_time_estimator.addDistance(hor_dist, direction, get_absolute_altitude_for_item(next_position_mission_item) - altitude_at_calculation_point);