diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index f5ce636c23..d468563597 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -58,6 +58,7 @@ void LoggedTopics::add_default_topics() add_topic("commander_state"); add_topic("config_overrides"); add_topic("cpuload"); + add_topic("distance_sensor_mode_change_request"); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 1a6cbf9775..b8093ca01c 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -73,7 +73,7 @@ public: virtual void on_activation() override; virtual void on_active() override; - bool isLanding(); + virtual bool isLanding(); protected: diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ec72881fc5..7fece40a4e 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -316,6 +317,7 @@ private: uORB::Publication _vehicle_cmd_pub{ORB_ID(vehicle_command)}; uORB::Publication _vehicle_roi_pub{ORB_ID(vehicle_roi)}; uORB::Publication _mode_completed_pub{ORB_ID(mode_completed)}; + uORB::PublicationData _distance_sensor_mode_change_request_pub{ORB_ID(distance_sensor_mode_change_request)}; orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */ @@ -336,6 +338,7 @@ private: position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */ vehicle_roi_s _vroi{}; /**< vehicle ROI */ + perf_counter_t _loop_perf; /**< loop performance counter */ Geofence _geofence; /**< class that handles the geofence */ @@ -400,6 +403,8 @@ private: void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result); + void publish_distance_sensor_mode_request(); + bool geofence_allows_position(const vehicle_global_position_s &pos); DEFINE_PARAMETERS( diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9a9f46e962..e46decdfb1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -110,6 +110,11 @@ Navigator::Navigator() : _mission_sub = orb_subscribe(ORB_ID(mission)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _distance_sensor_mode_change_request_pub.advertise(); + _distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time(); + _distance_sensor_mode_change_request_pub.get().req_mode = distance_sensor_mode_change_request_s::MODE_DISABLED; + _distance_sensor_mode_change_request_pub.update(); + // Update the timeout used in mission_block (which can't hold it's own parameters) _mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get()); @@ -898,6 +903,8 @@ void Navigator::run() publish_navigator_status(); + publish_distance_sensor_mode_request(); + _geofence.run(); perf_end(_loop_perf); @@ -1447,6 +1454,30 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) _vehicle_cmd_pub.publish(*vcmd); } +void Navigator::publish_distance_sensor_mode_request() +{ + // Send request to enable distance sensor when in the landing phase of a mission or RTL + if (((_navigation_mode == &_rtl) && _rtl.isLanding()) || ((_navigation_mode == &_mission) && _mission.isLanding())) { + + if (_distance_sensor_mode_change_request_pub.get().req_mode != + distance_sensor_mode_change_request_s::MODE_ENABLED) { + + _distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time(); + _distance_sensor_mode_change_request_pub.get().req_mode = + distance_sensor_mode_change_request_s::MODE_ENABLED; + _distance_sensor_mode_change_request_pub.update(); + } + + } else if (_distance_sensor_mode_change_request_pub.get().req_mode != + distance_sensor_mode_change_request_s::MODE_DISABLED) { + + _distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time(); + _distance_sensor_mode_change_request_pub.get().req_mode = + distance_sensor_mode_change_request_s::MODE_DISABLED; + _distance_sensor_mode_change_request_pub.update(); + } +} + void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result) { vehicle_command_ack_s command_ack = {}; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 63e2ab185f..cb33f350b7 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -304,6 +304,28 @@ void RTL::on_active() } } +bool RTL::isLanding() +{ + bool is_landing{false}; + + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + case RtlType::RTL_MISSION_FAST_REVERSE: + case RtlType::RTL_DIRECT_MISSION_LAND: + is_landing = _rtl_mission_type_handle->isLanding(); + break; + + case RtlType::RTL_DIRECT: + is_landing = _rtl_direct.isLanding(); + break; + + default: + break; + } + + return is_landing; +} + void RTL::setRtlTypeAndDestination() { diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index e7b720bd61..4ef13cb787 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -89,6 +89,8 @@ public: void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; } + bool isLanding(); + private: enum class DestinationType { DESTINATION_TYPE_HOME, diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 91e53dbe6b..aed7cf3e4c 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -103,6 +103,8 @@ public: void setRtlPosition(PositionYawSetpoint position, loiter_point_s loiter_pos); + bool isLanding() { return (_rtl_state != RTLState::IDLE) && (_rtl_state >= RTLState::LOITER_DOWN);}; + private: /** * @brief Return to launch state machine. diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index d7c42516e4..d78f6c7fa6 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -60,6 +60,12 @@ void RtlMissionFastReverse::on_inactive() _mission.current_seq : -1; } +void RtlMissionFastReverse::on_inactivation() +{ + MissionBase::on_inactivation(); + _in_landing_phase = false; +} + void RtlMissionFastReverse::on_activation() { _home_pos_sub.update(); @@ -120,6 +126,7 @@ void RtlMissionFastReverse::setActiveMissionItems() _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || num_found_items == 0) { handleLanding(new_work_item_type); + _in_landing_phase = true; } else { // convert mission item to a simple waypoint, keep loiter to alt @@ -131,6 +138,8 @@ void RtlMissionFastReverse::setActiveMissionItems() _mission_item.time_inside = 0.0f; pos_sp_triplet->previous = pos_sp_triplet->current; + + _in_landing_phase = false; } if (num_found_items > 0) { diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index f1301a5cca..168d20c2e3 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -58,6 +58,9 @@ public: void on_activation() override; void on_active() override; void on_inactive() override; + void on_inactivation() override; + + bool isLanding() override {return _in_landing_phase;}; rtl_time_estimate_s calc_rtl_time_estimate() override; @@ -68,5 +71,7 @@ private: int _mission_index_prior_rtl{-1}; + bool _in_landing_phase{false}; + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ };