From 0bf9c2a9b262a4c8569031f0f7e9ded432d2d4b3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Jun 2014 14:09:22 +0200 Subject: [PATCH] navigator: API changes, reparing to move manual modes to navigator, WIP --- src/modules/navigator/loiter.cpp | 21 ++++++---- src/modules/navigator/loiter.h | 4 +- src/modules/navigator/mission.cpp | 42 +++++++++---------- src/modules/navigator/mission.h | 6 +-- src/modules/navigator/mission_block.cpp | 53 +++++++++++------------- src/modules/navigator/mission_block.h | 4 +- src/modules/navigator/navigator.h | 4 +- src/modules/navigator/navigator_main.cpp | 11 ++--- src/modules/navigator/navigator_mode.cpp | 19 ++++----- src/modules/navigator/navigator_mode.h | 9 ++-- 10 files changed, 87 insertions(+), 86 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 5e7067b0e6..f827e70c96 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -36,6 +36,7 @@ * Helper class to loiter * * @author Julian Oes + * @author Anton Babushkin */ #include @@ -51,14 +52,13 @@ #include #include "loiter.h" +#include "navigator.h" Loiter::Loiter(Navigator *navigator, const char *name) : MissionBlock(navigator, name) { /* load initial params */ updateParams(); - /* initial reset */ - on_inactive(); } Loiter::~Loiter() @@ -71,16 +71,23 @@ Loiter::on_inactive() } void -Loiter::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +Loiter::on_activation() { - set_loiter_item(pos_sp_triplet); + /* set current mission item to loiter */ + set_loiter_item(&_mission_item); + + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + + _navigator->set_position_setpoint_triplet_updated(); } -bool -Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Loiter::on_active() { - return false; } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 5ce86d2a74..37ab57a078 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -56,9 +56,9 @@ public: virtual void on_inactive(); - virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); }; #endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ab99a6b7e0..1e86b1b6ca 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -54,8 +54,8 @@ #include #include -#include "navigator.h" #include "mission.h" +#include "navigator.h" Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), @@ -97,16 +97,14 @@ Mission::on_inactive() } void -Mission::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::on_activation() { - set_mission_items(pos_sp_triplet); + set_mission_items(); } -bool -Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Mission::on_active() { - bool updated = false; - /* check if anything has changed */ bool onboard_updated; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); @@ -122,18 +120,18 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) /* reset mission items if needed */ if (onboard_updated || offboard_updated) { - set_mission_items(pos_sp_triplet); - updated = true; + set_mission_items(); + + _navigator->set_position_setpoint_triplet_updated(); } /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); - set_mission_items(pos_sp_triplet); - updated = true; - } + set_mission_items(); - return updated; + _navigator->set_position_setpoint_triplet_updated(); + } } void @@ -223,8 +221,10 @@ Mission::advance_mission() } void -Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::set_mission_items() { + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + set_previous_pos_setpoint(pos_sp_triplet); /* try setting onboard mission item */ @@ -235,7 +235,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: onboard mission running"); } _mission_type = MISSION_TYPE_ONBOARD; - _navigator->set_can_loiter_at_sp(false); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); /* try setting offboard mission item */ } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { @@ -245,7 +245,8 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: offboard mission running"); } _mission_type = MISSION_TYPE_OFFBOARD; - _navigator->set_can_loiter_at_sp(false); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); + } else { if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_info(_navigator->get_mavlink_fd(), @@ -255,12 +256,15 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: no mission available"); } _mission_type = MISSION_TYPE_NONE; + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); - set_loiter_item(pos_sp_triplet); + set_loiter_item(&_mission_item); + pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + reset_mission_item_reached(); report_mission_finished(); } @@ -279,7 +283,6 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current &new_mission_item)) { /* convert the current mission item and set it valid */ mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); - current_pos_sp->valid = true; reset_mission_item_reached(); @@ -306,7 +309,6 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) { /* convert the current mission item and set it valid */ mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); - current_pos_sp->valid = true; reset_mission_item_reached(); @@ -330,7 +332,6 @@ Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - next_pos_sp->valid = true; return; } } @@ -358,7 +359,6 @@ Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - next_pos_sp->valid = true; return; } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 38a4f76128..ac293990f7 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -71,9 +71,9 @@ public: virtual void on_inactive(); - virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); private: /** @@ -94,7 +94,7 @@ private: /** * Set new mission items */ - void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet); + void set_mission_items(); /** * Try to set the current position setpoint from an onboard mission item diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index acf3ad5693..bf3c934a6b 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -58,8 +58,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _mission_item({0}), - _mission_item_valid(false) + _mission_item({0}) { } @@ -70,6 +69,10 @@ MissionBlock::~MissionBlock() bool MissionBlock::is_mission_item_reached() { + if (_mission_item.nav_cmd == NAV_CMD_IDLE) { + return false; + } + if (_mission_item.nav_cmd == NAV_CMD_LAND) { return _navigator->get_vstatus()->condition_landed; } @@ -83,7 +86,6 @@ MissionBlock::is_mission_item_reached() hrt_abstime now = hrt_absolute_time(); if (!_waypoint_position_reached) { - float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; @@ -209,43 +211,38 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_ } void -MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) +MissionBlock::set_loiter_item(struct mission_item_s *item) { if (_navigator->get_vstatus()->condition_landed) { /* landed, don't takeoff, but switch to IDLE mode */ - _mission_item.nav_cmd = NAV_CMD_IDLE; - - _navigator->set_can_loiter_at_sp(false); - - mavlink_log_info(_navigator->get_mavlink_fd(), "landed, switch to IDLE"); + item->nav_cmd = NAV_CMD_IDLE; } else { - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) { /* use current position setpoint */ - _mission_item.lat = pos_sp_triplet->current.lat; - _mission_item.lon = pos_sp_triplet->current.lon; - _mission_item.altitude = pos_sp_triplet->current.alt; + item->lat = pos_sp_triplet->current.lat; + item->lon = pos_sp_triplet->current.lon; + item->altitude = pos_sp_triplet->current.alt; } else { /* use current position */ - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = _navigator->get_global_position()->alt; + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + item->altitude = _navigator->get_global_position()->alt; } - _mission_item.altitude_is_relative = false; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = false; - _mission_item.origin = ORIGIN_ONBOARD; - - _navigator->set_can_loiter_at_sp(true); - mavlink_log_info(_navigator->get_mavlink_fd(), "switch to LOITER"); + item->altitude_is_relative = false; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 4f79238f46..22f42cc318 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -91,11 +91,9 @@ protected: /** * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position */ - // TODO remove argument, get position setpoint from navigator, add to arguments pointer to mission item instead - void set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet); + void set_loiter_item(struct mission_item_s *item); mission_item_s _mission_item; - bool _mission_item_valid; bool _waypoint_position_reached; bool _waypoint_yaw_reached; hrt_abstime _time_first_inside_orbit; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 184ecd365f..5dcd8859eb 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -103,6 +103,7 @@ public: * Setters */ void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } + void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } /** * Getters @@ -110,6 +111,7 @@ public: struct vehicle_status_s* get_vstatus() { return &_vstatus; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } + struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } Geofence& get_geofence() { return _geofence; } @@ -162,7 +164,7 @@ private: NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ - bool _update_triplet; /**< flags if position SP triplet needs to be published */ + bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 266114e385..bb48db829b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -121,7 +121,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _update_triplet(false), + _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD") { @@ -371,20 +371,21 @@ Navigator::task_main() /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { - _update_triplet = _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i], &_pos_sp_triplet); + _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid */ if (_navigation_mode == nullptr) { + // TODO publish empty sp only once _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; - _update_triplet = true; + _pos_sp_triplet_updated = true; } - if (_update_triplet) { + if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); - _update_triplet = false; + _pos_sp_triplet_updated = false; } perf_end(_loop_perf); diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 6361ea9c8a..f432156652 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -40,6 +40,7 @@ */ #include "navigator_mode.h" +#include "navigator.h" NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : SuperBlock(NULL, name), @@ -56,18 +57,17 @@ NavigatorMode::~NavigatorMode() { } -bool -NavigatorMode::run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet) { +void +NavigatorMode::run(bool active) { if (active) { if (_first_run) { /* first run */ _first_run = false; - on_activation(pos_sp_triplet); - return true; + on_activation(); } else { /* periodic updates when active */ - on_active(pos_sp_triplet); + on_active(); } } else { @@ -83,14 +83,13 @@ NavigatorMode::on_inactive() } void -NavigatorMode::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +NavigatorMode::on_activation() { /* invalidate position setpoint by default */ - pos_sp_triplet->current.valid = false; + _navigator->get_position_setpoint_triplet()->current.valid = false; } -bool -NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +NavigatorMode::on_active() { - return false; } diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 5c36af1fe9..a7ba79bba7 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -66,7 +66,7 @@ public: */ virtual ~NavigatorMode(); - bool run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet); + void run(bool active); /** * This function is called while the mode is inactive @@ -76,15 +76,12 @@ public: /** * This function is called one time when mode become active, poos_sp_triplet must be initialized here */ - virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); /** * This function is called while the mode is active - * - * @param position setpoint triplet to set - * @return true if position setpoint triplet has been changed */ - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); protected: Navigator *_navigator;