diff options
-rw-r--r-- | src/modules/navigator/mission.cpp | 37 | ||||
-rw-r--r-- | src/modules/navigator/mission.h | 2 | ||||
-rw-r--r-- | src/modules/navigator/navigator.h | 2 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 9 | ||||
-rw-r--r-- | src/modules/navigator/rtl.cpp | 4 |
5 files changed, 34 insertions, 20 deletions
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index f3a86666f..7e02f8c15 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -50,6 +50,7 @@ #include <geo/geo.h> #include <uORB/uORB.h> +#include <uORB/topics/mission.h> #include <uORB/topics/mission_result.h> #include "navigator.h" @@ -61,8 +62,6 @@ Mission::Mission(Navigator *navigator) : _navigator(navigator), _first_run(true), _param_onboard_enabled(this, "ONBOARD_EN"), - _onboard_mission_sub(-1), - _offboard_mission_sub(-1), _onboard_mission({0}), _offboard_mission({0}), _mission_item({0}), @@ -74,6 +73,9 @@ Mission::Mission(Navigator *navigator) : updateParams(); /* set initial mission items */ reset(); + + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); } Mission::~Mission() @@ -89,9 +91,14 @@ Mission::reset() bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { + /* check if anything has changed */ + bool onboard_updated = false; //is_onboard_mission_updated(); + bool offboard_updated = is_offboard_mission_updated(); + bool updated = false; - /* check if anything has changed, and reset mission items if needed */ - if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) { + + /* reset mission items if needed */ + if (onboard_updated || offboard_updated) { set_mission_items(pos_sp_triplet); updated = true; _first_run = false; @@ -229,7 +236,7 @@ Mission::is_onboard_mission_updated() bool updated; orb_check(_onboard_mission_sub, &updated); - if (!updated) { + if (!updated && !_first_run) { return false; } @@ -244,12 +251,17 @@ bool Mission::is_offboard_mission_updated() { bool updated; + warnx("sub: %d", _offboard_mission_sub); orb_check(_offboard_mission_sub, &updated); - if (!updated) { + if (!updated && !_first_run) { + warnx("not updated"); return false; } - - if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) { + struct mission_s offboard_mission; + int ret = orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission); + warnx("ret: %d", ret); + if (ret == OK) { + warnx("copy new offboard mission"); /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -263,8 +275,10 @@ Mission::is_offboard_mission_updated() missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t)_offboard_mission.count, - _navigator->get_geofence()); + _navigator->get_geofence(), + _navigator->get_home_position()->alt); } else { + warnx("no success with orb_copy"); _offboard_mission.count = 0; _offboard_mission.current_index = 0; } @@ -341,7 +355,9 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current bool Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { + warnx("try offboard mission: %d, %d", _offboard_mission.current_index, _offboard_mission.count ); if (_offboard_mission.current_index < (int)_offboard_mission.count) { + warnx("theoretically possible"); dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -356,8 +372,11 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren report_current_offboard_mission_item(); return true; + } else { + warnx("read fail"); } } + warnx("failed with offboard mission"); return false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index e86dd25bb..65a0991b5 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -47,9 +47,11 @@ #include <dataman/dataman.h> +#include <uORB/uORB.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/mission.h> #include <uORB/topics/mission_result.h> #include "mission_feasibility_checker.h" diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 1838fe32b..927e77391 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -103,8 +103,6 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ - int _offboard_mission_sub; /**< notification of offboard mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f91032196..9dd253127 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -103,8 +103,6 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _params_sub(-1), - _offboard_mission_sub(-1), - _onboard_mission_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), _pos_sp_triplet_pub(-1), @@ -248,8 +246,6 @@ Navigator::task_main() /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); - _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -360,6 +356,8 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: + _mission.reset(); + _rtl.reset(); break; case NAVIGATION_STATE_AUTO_MISSION: _update_triplet = _mission.update(&_pos_sp_triplet); @@ -660,8 +658,6 @@ Navigator::start_land() _update_triplet = true; return true; } -#endif -#if 0 bool Navigator::check_mission_item_reached() { @@ -766,6 +762,7 @@ Navigator::reset_reached() _waypoint_yaw_reached = false; } +#endif void Navigator::publish_position_setpoint_triplet() { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 9d7886aa6..dc87b0521 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -57,9 +57,7 @@ RTL::RTL(Navigator *navigator) : _rtl_state(RTL_STATE_NONE), _home_position({}), _loiter_radius(50), - _acceptance_radius(50), - _param_return_alt(this, "RETURN_ALT"), - _param_descend_alt(this, "DESCEND_ALT"), + _acceptance_radius(50) _param_land_delay(this, "LAND_DELAY") { /* load initial params */ |