From ee517b2f9233a99c1205504cbcab6a608d939887 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 30 Sep 2018 14:01:55 -0400 Subject: [PATCH] navigator initialization and mode change improvements --- src/modules/navigator/land.cpp | 9 +- src/modules/navigator/loiter.cpp | 32 ++-- src/modules/navigator/loiter.h | 11 +- src/modules/navigator/mission.h | 8 - src/modules/navigator/mission_block.cpp | 22 ++- src/modules/navigator/mission_block.h | 6 +- src/modules/navigator/navigator.h | 8 +- src/modules/navigator/navigator_main.cpp | 198 +++++++++-------------- src/modules/navigator/takeoff.cpp | 24 ++- src/modules/navigator/takeoff.h | 4 + 10 files changed, 143 insertions(+), 179 deletions(-) diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 7a17c8cd6bfd..82e13dceba08 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -56,7 +56,7 @@ Land::on_activation() reset_mission_item_reached(); /* convert mission item to current setpoint */ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous.valid = false; mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); @@ -73,19 +73,22 @@ Land::on_active() /* for VTOL update landing location during back transition */ if (_navigator->get_vstatus()->is_vtol && _navigator->get_vstatus()->in_transition_mode) { - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + _navigator->set_position_setpoint_triplet_updated(); } if (_navigator->get_land_detected()->landed) { + _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); set_idle_item(&_mission_item); - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index ba14e09016fa..0391534eeb0b 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -57,7 +57,7 @@ Loiter::on_inactive() void Loiter::on_activation() { - if (_navigator->get_reposition_triplet()->current.valid) { + if (_reposition.valid) { reposition(); } else { @@ -68,7 +68,7 @@ Loiter::on_activation() void Loiter::on_active() { - if (_navigator->get_reposition_triplet()->current.valid) { + if (_reposition.valid) { reposition(); } @@ -98,8 +98,6 @@ Loiter::set_loiter_position() return; } - _loiter_pos_set = true; - // set current mission item to loiter set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt()); @@ -113,6 +111,8 @@ Loiter::set_loiter_position() _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); + + _loiter_pos_set = true; } void @@ -123,25 +123,29 @@ Loiter::reposition() return; } - struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet(); - - if (rep->current.valid) { + if (_reposition.valid) { // set loiter position based on reposition command - // convert mission item to current setpoint - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->current.velocity_valid = false; - pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // previous: current position pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat; pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon; pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt; - memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current)); + pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw; + pos_sp_triplet->previous.valid = true; + + // current: use reposition + pos_sp_triplet->current = _reposition; + pos_sp_triplet->current.velocity_valid = false; + + // next: invalid pos_sp_triplet->next.valid = false; _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); - // mark this as done - memset(rep, 0, sizeof(*rep)); + // clear the reposition to mark as done + _reposition = position_setpoint_s{}; } } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 7469bcbb74e0..2d0e930d47c7 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -55,14 +55,7 @@ class Loiter : public MissionBlock, public ModuleParams void on_activation() override; void on_active() override; - // TODO: share this with mission - enum mission_yaw_mode { - MISSION_YAWMODE_NONE = 0, - MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, - MISSION_YAWMODE_FRONT_TO_HOME = 2, - MISSION_YAWMODE_BACK_TO_HOME = 3, - MISSION_YAWMODE_MAX = 4 - }; + position_setpoint_s &get_reposition() { return _reposition; } private: /** @@ -76,5 +69,7 @@ class Loiter : public MissionBlock, public ModuleParams */ void set_loiter_position(); + position_setpoint_s _reposition{}; /**< position setpoint for non-mission direct position command */ + bool _loiter_pos_set{false}; }; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index d9b8eea23c02..1ab4bf5a1641 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -80,14 +80,6 @@ class Mission : public MissionBlock, public ModuleParams MISSION_ALTMODE_FOH = 1 }; - enum mission_yaw_mode { - MISSION_YAWMODE_NONE = 0, - MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, - MISSION_YAWMODE_FRONT_TO_HOME = 2, - MISSION_YAWMODE_BACK_TO_HOME = 3, - MISSION_YAWMODE_MAX = 4 - }; - bool set_current_offboard_mission_index(uint16_t index); bool land_start(); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 5e8d1b2e79d4..94813d26325f 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -59,6 +59,14 @@ using matrix::wrap_pi; MissionBlock::MissionBlock(Navigator *navigator) : NavigatorMode(navigator) { + _mission_item.lat = (double)NAN; + _mission_item.lon = (double)NAN; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; } bool @@ -469,10 +477,14 @@ MissionBlock::issue_command(const mission_item_s &item) } float -MissionBlock::get_time_inside(const struct mission_item_s &item) +MissionBlock::get_time_inside(const mission_item_s &item) const { - if (item.nav_cmd != NAV_CMD_TAKEOFF) { - return item.time_inside; + if ((item.nav_cmd == NAV_CMD_WAYPOINT && _navigator->get_vstatus()->is_rotary_wing) || + item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item.nav_cmd == NAV_CMD_DELAY) { + + // TODO: set appropriate upper limit + return math::constrain(item.time_inside, 0.0f, 3600.0f); } return 0.0f; @@ -502,7 +514,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->lat = item.lat; sp->lon = item.lon; - sp->alt = item.altitude_is_relative ? item.altitude + _navigator->get_home_position()->alt : item.altitude; + sp->alt = get_absolute_altitude_for_item(item); sp->yaw = item.yaw; sp->yaw_valid = PX4_ISFINITE(item.yaw); sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) : @@ -728,7 +740,7 @@ MissionBlock::mission_apply_limitation(mission_item_s &item) } float -MissionBlock::get_absolute_altitude_for_item(struct mission_item_s &mission_item) const +MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const { if (mission_item.altitude_is_relative) { return mission_item.altitude + _navigator->get_home_position()->alt; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 052b805986a9..9561b85ffc13 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -115,13 +115,13 @@ class MissionBlock : public NavigatorMode /** * General function used to adjust the mission item based on vehicle specific limitations */ - void mission_apply_limitation(mission_item_s &item); + void mission_apply_limitation(mission_item_s &item); void issue_command(const mission_item_s &item); - float get_time_inside(const struct mission_item_s &item); + float get_time_inside(const mission_item_s &item) const ; - float get_absolute_altitude_for_item(struct mission_item_s &mission_item) const; + float get_absolute_altitude_for_item(const mission_item_s &mission_item) const; mission_item_s _mission_item{}; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 8dc30815d7a1..d29902578a1e 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -84,7 +84,8 @@ class Navigator : public ModuleBase, public ModuleParams { public: Navigator(); - virtual ~Navigator() = default; + ~Navigator() override; + Navigator(const Navigator &) = delete; Navigator operator=(const Navigator &) = delete; @@ -149,8 +150,6 @@ class Navigator : public ModuleBase, public ModuleParams struct home_position_s *get_home_position() { return &_home_pos; } struct mission_result_s *get_mission_result() { return &_mission_result; } struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; } - struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; } - struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; } struct vehicle_global_position_s *get_global_position() { return &_global_pos; } struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; } struct vehicle_local_position_s *get_local_position() { return &_local_pos; } @@ -324,8 +323,6 @@ class Navigator : public ModuleBase, public ModuleParams // Publications geofence_result_s _geofence_result{}; position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */ - position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */ - 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 */ @@ -335,7 +332,6 @@ class Navigator : public ModuleBase, public ModuleParams bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */ - bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */ bool _mission_result_updated{false}; /**< flags if mission result has seen an update */ NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8356ec5b11c2..ea8fe71d567d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -113,6 +113,35 @@ Navigator::Navigator() : _handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS"); _handle_reverse_delay = param_find("VT_B_REV_DEL"); + + // subscriptions + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + _pos_ctrl_landing_status_sub = orb_subscribe(ORB_ID(position_controller_landing_status)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + _home_pos_sub = orb_subscribe(ORB_ID(home_position)); + _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); + _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); + _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); + _traffic_sub = orb_subscribe(ORB_ID(transponder_report)); + + reset_triplets(); +} + +Navigator::~Navigator() +{ + orb_unsubscribe(_global_pos_sub); + orb_unsubscribe(_local_pos_sub); + orb_unsubscribe(_gps_pos_sub); + orb_unsubscribe(_pos_ctrl_landing_status_sub); + orb_unsubscribe(_vstatus_sub); + orb_unsubscribe(_land_detected_sub); + orb_unsubscribe(_home_pos_sub); + orb_unsubscribe(_offboard_mission_sub); + orb_unsubscribe(_param_update_sub); + orb_unsubscribe(_vehicle_command_sub); } void @@ -189,19 +218,6 @@ Navigator::run() _geofence.loadFromFile(GEOFENCE_FILENAME); } - /* do subscriptions */ - _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - _pos_ctrl_landing_status_sub = orb_subscribe(ORB_ID(position_controller_landing_status)); - _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); - _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); - _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); - _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); - _traffic_sub = orb_subscribe(ORB_ID(transponder_report)); - /* copy all topics first time */ vehicle_status_update(); vehicle_land_detected_update(); @@ -316,51 +332,36 @@ Navigator::run() } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { - position_setpoint_triplet_s *rep = get_reposition_triplet(); + position_setpoint_s &rep = _loiter.get_reposition(); position_setpoint_triplet_s *curr = get_position_setpoint_triplet(); - // store current position as previous position and goal as next - rep->previous.yaw = get_global_position()->yaw; - rep->previous.lat = get_global_position()->lat; - rep->previous.lon = get_global_position()->lon; - rep->previous.alt = get_global_position()->alt; - - rep->current.loiter_radius = get_loiter_radius(); - rep->current.loiter_direction = 1; - rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; - - // If no argument for ground speed, use default value. - if (cmd.param1 <= 0 || !PX4_ISFINITE(cmd.param1)) { - rep->current.cruising_speed = get_cruising_speed(); - - } else { - rep->current.cruising_speed = cmd.param1; - } - - rep->current.cruising_throttle = get_cruising_throttle(); - rep->current.acceptance_radius = get_acceptance_radius(); + rep.loiter_radius = get_loiter_radius(); + rep.loiter_direction = 1; + rep.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + rep.cruising_speed = get_cruising_speed(); + rep.cruising_throttle = get_cruising_throttle(); // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { - rep->current.yaw = cmd.param4; - rep->current.yaw_valid = true; + rep.yaw = cmd.param4; + rep.yaw_valid = true; } else { - rep->current.yaw = NAN; - rep->current.yaw_valid = false; + rep.yaw = NAN; + rep.yaw_valid = false; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { // Position change with optional altitude change - rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; - rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; + rep.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; + rep.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; if (PX4_ISFINITE(cmd.param7)) { - rep->current.alt = cmd.param7; + rep.alt = cmd.param7; } else { - rep->current.alt = get_global_position()->alt; + rep.alt = get_global_position()->alt; } } else if (PX4_ISFINITE(cmd.param7) && curr->current.valid @@ -368,59 +369,48 @@ Navigator::run() && PX4_ISFINITE(curr->current.lon)) { // Altitude without position change - rep->current.lat = curr->current.lat; - rep->current.lon = curr->current.lon; - rep->current.alt = cmd.param7; + rep.lat = curr->current.lat; + rep.lon = curr->current.lon; + rep.alt = cmd.param7; } else { // All three set to NaN - hold in current position - rep->current.lat = get_global_position()->lat; - rep->current.lon = get_global_position()->lon; - rep->current.alt = get_global_position()->alt; + rep.lat = get_global_position()->lat; + rep.lon = get_global_position()->lon; + rep.alt = get_global_position()->alt; } - rep->previous.valid = true; - rep->current.valid = true; - rep->next.valid = false; + rep.valid = true; // CMD_DO_REPOSITION is acknowledged by commander } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { - position_setpoint_triplet_s *rep = get_takeoff_triplet(); - - // store current position as previous position and goal as next - rep->previous.yaw = get_global_position()->yaw; - rep->previous.lat = get_global_position()->lat; - rep->previous.lon = get_global_position()->lon; - rep->previous.alt = get_global_position()->alt; + position_setpoint_s &rep = _takeoff.get_reposition(); - rep->current.loiter_radius = get_loiter_radius(); - rep->current.loiter_direction = 1; - rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; + rep.loiter_radius = get_loiter_radius(); + rep.loiter_direction = 1; + rep.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; if (home_position_valid()) { - rep->current.yaw = cmd.param4; - rep->previous.valid = true; + rep.yaw = cmd.param4; } else { - rep->current.yaw = get_local_position()->yaw; - rep->previous.valid = false; + rep.yaw = get_local_position()->yaw; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { - rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; - rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; + rep.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; + rep.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { // If one of them is non-finite, reset both - rep->current.lat = (double)NAN; - rep->current.lon = (double)NAN; + rep.lat = (double)NAN; + rep.lon = (double)NAN; } - rep->current.alt = cmd.param7; + rep.alt = cmd.param7; - rep->current.valid = true; - rep->next.valid = false; + rep.valid = true; // CMD_NAV_TAKEOFF is acknowledged by commander @@ -475,7 +465,8 @@ Navigator::run() || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE) { - _vroi = {}; + + _vroi = vehicle_roi_s{}; switch (cmd.command) { case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI: @@ -562,25 +553,20 @@ Navigator::run() switch (_vstatus.nav_state) { case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - _pos_sp_triplet_published_invalid_once = false; - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_NORMAL); navigation_mode_new = &_mission; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_loiter; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_rcLoss; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: { - _pos_sp_triplet_published_invalid_once = false; const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; @@ -671,43 +657,35 @@ Navigator::run() } case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_takeoff; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_land; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_precland; _precland.set_mode(PrecLandMode::Required); break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_land; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_dataLinkLoss; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_engineFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_gpsFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_follow_target; break; @@ -719,7 +697,6 @@ Navigator::run() case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: case vehicle_status_s::NAVIGATION_STATE_STAB: default: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = nullptr; _can_loiter_at_sp = false; break; @@ -730,16 +707,18 @@ Navigator::run() /* we have a new navigation mode: reset triplet */ if (_navigation_mode != navigation_mode_new) { - // We don't reset the triplet if we just did an auto-takeoff and are now - // going to loiter. Otherwise, we lose the takeoff altitude and end up lower - // than where we wanted to go. - // - // FIXME: a better solution would be to add reset where they are needed and remove - // this general reset here. - if (!(_navigation_mode == &_takeoff && - navigation_mode_new == &_loiter)) { - reset_triplets(); + + // if switching to loiter after an auto-takeoff use the same position + if ((_navigation_mode == &_takeoff) && (navigation_mode_new == &_loiter)) { + + if (_pos_sp_triplet.current.valid && (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { + + _loiter.get_reposition() = _pos_sp_triplet.current; + _loiter.get_reposition().type = position_setpoint_s::SETPOINT_TYPE_LOITER; + } } + + reset_triplets(); } _navigation_mode = navigation_mode_new; @@ -754,17 +733,16 @@ Navigator::run() !((_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) || (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION))) { + const uint8_t type_prev = _pos_sp_triplet.current.type; + _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.next.valid = false; - } - - /* if nothing is running, set position setpoint triplet invalid once */ - if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { - _pos_sp_triplet_published_invalid_once = true; - reset_triplets(); + if (_pos_sp_triplet.current.type != type_prev) { + _pos_sp_triplet_updated = true; + } } if (_pos_sp_triplet_updated) { @@ -777,17 +755,6 @@ Navigator::run() perf_end(_loop_perf); } - - orb_unsubscribe(_global_pos_sub); - orb_unsubscribe(_local_pos_sub); - orb_unsubscribe(_gps_pos_sub); - orb_unsubscribe(_pos_ctrl_landing_status_sub); - orb_unsubscribe(_vstatus_sub); - orb_unsubscribe(_land_detected_sub); - orb_unsubscribe(_home_pos_sub); - orb_unsubscribe(_offboard_mission_sub); - orb_unsubscribe(_param_update_sub); - orb_unsubscribe(_vehicle_command_sub); } int Navigator::task_spawn(int argc, char *argv[]) @@ -830,11 +797,6 @@ Navigator::print_status() void Navigator::publish_position_setpoint_triplet() { - // do not publish an invalid setpoint - if (!_pos_sp_triplet.current.valid) { - return; - } - _pos_sp_triplet.timestamp = hrt_absolute_time(); /* lazily publish the position setpoint triplet only once available */ diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 7e737e07e5a2..86c6dc8c21bd 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -55,9 +55,7 @@ Takeoff::on_activation() void Takeoff::on_active() { - struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet(); - - if (rep->current.valid) { + if (_reposition.valid) { // reset the position set_takeoff_position(); @@ -77,8 +75,6 @@ Takeoff::on_active() void Takeoff::set_takeoff_position() { - struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet(); - float abs_altitude = 0.0f; float min_abs_altitude; @@ -91,8 +87,8 @@ Takeoff::set_takeoff_position() } // Use altitude if it has been set. If home position is invalid use min_abs_altitude - if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_position_valid()) { - abs_altitude = rep->current.alt; + if (_reposition.valid && PX4_ISFINITE(_reposition.alt) && _navigator->home_position_valid()) { + abs_altitude = _reposition.alt; // If the altitude suggestion is lower than home + minimum clearance, raise it and complain. if (abs_altitude < min_abs_altitude) { @@ -132,20 +128,20 @@ Takeoff::set_takeoff_position() pos_sp_triplet->current.yaw_valid = true; pos_sp_triplet->next.valid = false; - if (rep->current.valid) { + if (_reposition.valid) { // Go on and check which changes had been requested - if (PX4_ISFINITE(rep->current.yaw)) { - pos_sp_triplet->current.yaw = rep->current.yaw; + if (PX4_ISFINITE(_reposition.yaw)) { + pos_sp_triplet->current.yaw = _reposition.yaw; } - if (PX4_ISFINITE(rep->current.lat) && PX4_ISFINITE(rep->current.lon)) { - pos_sp_triplet->current.lat = rep->current.lat; - pos_sp_triplet->current.lon = rep->current.lon; + if (PX4_ISFINITE(_reposition.lat) && PX4_ISFINITE(_reposition.lon)) { + pos_sp_triplet->current.lat = _reposition.lat; + pos_sp_triplet->current.lon = _reposition.lon; } // mark this as done - memset(rep, 0, sizeof(*rep)); + _reposition = position_setpoint_s{}; } _navigator->set_can_loiter_at_sp(true); diff --git a/src/modules/navigator/takeoff.h b/src/modules/navigator/takeoff.h index 6fb5d8ea192a..fe8474932eb6 100644 --- a/src/modules/navigator/takeoff.h +++ b/src/modules/navigator/takeoff.h @@ -52,7 +52,11 @@ class Takeoff : public MissionBlock void on_activation() override; void on_active() override; + position_setpoint_s &get_reposition() { return _reposition; } + private: void set_takeoff_position(); + + position_setpoint_s _reposition{}; /**< position setpoint for non-mission direct position command */ };