diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d3b8c3bff3..2c7bd166e7 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1018,6 +1018,12 @@ Mission::heading_sp_update() point_to_latlon[0] = _navigator->get_home_position()->lat; point_to_latlon[1] = _navigator->get_home_position()->lon; + } else if (_param_yawmode.get() == MISSION_YAWMODE_TO_ROI + && _navigator->get_vroi()->mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) { + /* target location is ROI */ + point_to_latlon[0] = _navigator->get_vroi()->lat; + point_to_latlon[1] = _navigator->get_vroi()->lon; + } else { /* target location is next (current) waypoint */ point_to_latlon[0] = pos_sp_triplet->current.lat; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index e77e308473..572e64ea51 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -61,6 +61,7 @@ #include #include #include +#include #include class Navigator; @@ -86,7 +87,8 @@ public: MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, MISSION_YAWMODE_FRONT_TO_HOME = 2, MISSION_YAWMODE_BACK_TO_HOME = 3, - MISSION_YAWMODE_MAX = 4 + MISSION_YAWMODE_TO_ROI = 4, + MISSION_YAWMODE_MAX = 5 }; bool set_current_offboard_mission_index(unsigned index); diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 3faebd82fa..c6f4593583 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -125,6 +125,7 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 1); * @value 1 Heading towards waypoint * @value 2 Heading towards home * @value 3 Heading away from home + * @value 4 Heading towards ROI * @group Mission */ PARAM_DEFINE_INT32(MIS_YAWMODE, 1); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 2b3939d724..ba1b0830c6 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -135,6 +135,7 @@ public: struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; } struct vehicle_local_position_s *get_local_position() { return &_local_pos; } struct vehicle_status_s *get_vstatus() { return &_vstatus; } + struct vehicle_roi_s *get_vroi() { return &_vroi; } bool home_position_valid() { return (_home_pos.timestamp > 0); } @@ -237,6 +238,7 @@ private: int _sensor_combined_sub{-1}; /**< sensor combined subscription */ int _vehicle_command_sub{-1}; /**< vehicle commands (onboard and offboard) */ int _vstatus_sub{-1}; /**< vehicle status subscription */ + int _vehicle_roi_sub{-1}; /**< vehicle ROI subscription */ orb_advert_t _geofence_result_pub{nullptr}; orb_advert_t _mission_result_pub{nullptr}; @@ -257,6 +259,7 @@ private: vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */ vehicle_local_position_s _local_pos{}; /**< local vehicle position */ vehicle_status_s _vstatus{}; /**< vehicle status */ + vehicle_roi_s _vroi{}; /**< vehicle ROI */ int _mission_instance_count{-1}; /**< instance count for the current mission */ @@ -304,6 +307,7 @@ private: void sensor_combined_update(); void vehicle_land_detected_update(); void vehicle_status_update(); + void vehicle_roi_update(); /** * Shim for calling task_main from task_create. diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e5b40fe27a..017c377128 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -213,6 +213,12 @@ Navigator::params_update() } } +void +Navigator::vehicle_roi_update() +{ + orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &_vroi); +} + void Navigator::task_main_trampoline(int argc, char *argv[]) { @@ -246,6 +252,7 @@ Navigator::task_main() _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); + _vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi)); /* copy all topics first time */ vehicle_status_update(); @@ -257,6 +264,7 @@ Navigator::task_main() home_position_update(true); fw_pos_ctrl_status_update(true); params_update(); + vehicle_roi_update(); /* wakeup source(s) */ px4_pollfd_struct_t fds[1] = {}; @@ -357,6 +365,13 @@ Navigator::task_main() home_position_update(); } + /* ROI updated */ + orb_check(_vehicle_roi_sub, &updated); + + if (updated) { + vehicle_roi_update(); + } + /* vehicle_command updated */ orb_check(_vehicle_command_sub, &updated);