From 56b028148bcb9b2fa4d6b658307fece64688eb19 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 20 Apr 2017 11:24:55 -0400 Subject: [PATCH] Navigator move get_time_inside and cleanup (#7062) --- src/modules/navigator/mission.cpp | 55 ++--- src/modules/navigator/mission.h | 75 ++++--- src/modules/navigator/mission_block.cpp | 77 +++---- src/modules/navigator/mission_block.h | 52 +++-- src/modules/navigator/navigator.h | 252 +++++++++-------------- src/modules/navigator/navigator_main.cpp | 130 +++--------- src/modules/navigator/navigator_mode.cpp | 4 - src/modules/navigator/navigator_mode.h | 21 +- src/modules/navigator/rtl.cpp | 4 +- 9 files changed, 234 insertions(+), 436 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 6dd61c8b01..9a44771d4e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -44,28 +44,20 @@ * @author Sander Smeets */ -#include -#include -#include -#include -#include +#include "mission.h" +#include "navigator.h" #include - #include #include #include #include #include #include - #include #include #include -#include "mission.h" -#include "navigator.h" - Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _param_onboard_enabled(this, "MIS_ONBOARD_EN", false), @@ -75,28 +67,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_yawmode(this, "MIS_YAWMODE", false), _param_force_vtol(this, "NAV_FORCE_VT", false), _param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false), - _onboard_mission{}, - _offboard_mission{}, - _current_onboard_mission_index(-1), - _current_offboard_mission_index(-1), - _need_takeoff(true), - _mission_type(MISSION_TYPE_NONE), - _inited(false), - _home_inited(false), - _need_mission_reset(false), - _missionFeasibilityChecker(), - _min_current_sp_distance_xy(FLT_MAX), - _distance_current_previous(0.0f), - _work_item_type(WORK_ITEM_TYPE_DEFAULT) + _missionFeasibilityChecker() { - /* load initial params */ updateParams(); } -Mission::~Mission() -{ -} - void Mission::on_inactive() { @@ -136,7 +111,7 @@ Mission::on_inactive() } else { /* load missions from storage */ - mission_s mission_state; + mission_s mission_state = {}; dm_lock(DM_KEY_MISSION_STATE); @@ -221,6 +196,7 @@ Mission::on_active() } } else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) { + altitude_sp_foh_update(); } else { @@ -230,7 +206,6 @@ Mission::on_active() } } - /* check if a cruise speed change has been commanded */ if (_mission_type != MISSION_TYPE_NONE) { cruising_speed_sp_update(); @@ -251,10 +226,10 @@ Mission::on_active() do_abort_landing(); } - } -bool Mission::set_current_offboard_mission_index(unsigned index) +bool +Mission::set_current_offboard_mission_index(unsigned index) { if (index < _offboard_mission.count) { @@ -275,7 +250,7 @@ bool Mission::set_current_offboard_mission_index(unsigned index) return false; } -unsigned +int Mission::find_offboard_land_start() { /* find the first MAV_CMD_DO_LAND_START and return the index @@ -433,11 +408,11 @@ Mission::advance_mission() float Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item) { - if (_mission_item.altitude_is_relative) { - return _mission_item.altitude + _navigator->get_home_position()->alt; + if (mission_item.altitude_is_relative) { + return mission_item.altitude + _navigator->get_home_position()->alt; } else { - return _mission_item.altitude; + return mission_item.altitude; } } @@ -544,6 +519,7 @@ Mission::set_mission_items() if (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() == 1 && !_navigator->get_vstatus()->is_rotary_wing && _navigator->get_vstatus()->is_vtol) { + _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; } @@ -825,7 +801,7 @@ Mission::set_mission_items() set_current_offboard_mission_item(); } - if (_mission_item.autocontinue && Navigator::get_time_inside(_mission_item) < FLT_EPSILON) { + if (_mission_item.autocontinue && get_time_inside(_mission_item) < FLT_EPSILON) { /* try to process next mission item */ if (has_next_position_item) { /* got next mission item, update setpoint triplet */ @@ -994,7 +970,7 @@ Mission::heading_sp_update() } /* set yaw angle for the waypoint if a loiter time has been specified */ - if (_waypoint_position_reached && Navigator::get_time_inside(_mission_item) > FLT_EPSILON) { + if (_waypoint_position_reached && get_time_inside(_mission_item) > FLT_EPSILON) { // XXX: should actually be param4 from mission item // at the moment it will just keep the heading it has //_mission_item.yaw = _on_arrival_yaw; @@ -1054,7 +1030,6 @@ Mission::heading_sp_update() _navigator->set_position_setpoint_triplet_updated(); } - void Mission::altitude_sp_foh_update() { @@ -1313,7 +1288,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss void Mission::save_offboard_mission_state() { - mission_s mission_state; + mission_s mission_state = {}; /* lock MISSION_STATE item */ dm_lock(DM_KEY_MISSION_STATE); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 61645b6701..8a13e9376b 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -45,24 +45,23 @@ #ifndef NAVIGATOR_MISSION_H #define NAVIGATOR_MISSION_H -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "navigator_mode.h" #include "mission_block.h" #include "mission_feasibility_checker.h" +#include "navigator_mode.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include class Navigator; @@ -70,14 +69,11 @@ class Mission : public MissionBlock { public: Mission(Navigator *navigator, const char *name); + ~Mission() override = default; - virtual ~Mission(); - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); + void on_inactive() override; + void on_activation() override; + void on_active() override; enum mission_altitude_mode { MISSION_ALTMODE_ZOH = 0, @@ -94,7 +90,7 @@ public: bool set_current_offboard_mission_index(unsigned index); - unsigned find_offboard_land_start(); + int find_offboard_land_start(); private: /** @@ -245,28 +241,28 @@ private: control::BlockParamInt _param_force_vtol; control::BlockParamFloat _param_fw_climbout_diff; - struct mission_s _onboard_mission; - struct mission_s _offboard_mission; + struct mission_s _onboard_mission {}; + struct mission_s _offboard_mission {}; - int _current_onboard_mission_index; - int _current_offboard_mission_index; - bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ + int _current_onboard_mission_index{-1}; + int _current_offboard_mission_index{-1}; + bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ enum { MISSION_TYPE_NONE, MISSION_TYPE_ONBOARD, MISSION_TYPE_OFFBOARD - } _mission_type; + } _mission_type{MISSION_TYPE_NONE}; - bool _inited; - bool _home_inited; - bool _need_mission_reset; + bool _inited{false}; + bool _home_inited{false}; + bool _need_mission_reset{false}; MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */ - float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */ + float _min_current_sp_distance_xy{FLT_MAX}; /**< minimum distance which was achieved to the current waypoint */ - float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, + float _distance_current_previous{0.0f}; /**< distance from previous to current sp in pos_sp_triplet, only use if current and previous are valid */ enum work_item_type { @@ -274,11 +270,10 @@ private: WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ - WORK_ITEM_TYPE_CMD_BEFORE_MOVE, /**< */ - WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, /**< */ - WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND, /**< */ - WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION /**< */ - } _work_item_type; /**< current type of work to do (sub mission item) */ + WORK_ITEM_TYPE_CMD_BEFORE_MOVE, + WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, + WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION + } _work_item_type{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ }; #endif diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index de3ef25447..8bed8a52a1 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -40,41 +40,24 @@ * @author Andreas Antener */ -#include -#include -#include -#include +#include "mission_block.h" +#include "navigator.h" + #include #include -#include #include #include #include - #include #include #include #include -#include "navigator.h" -#include "mission_block.h" - -actuator_controls_s actuators; -orb_advert_t actuator_pub_fd; MissionBlock::MissionBlock(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), - _mission_item{}, - _waypoint_position_reached(false), - _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _action_start(0), - _time_wp_reached(0), - _actuators{}, - _actuator_pub(nullptr), - _cmd_pub(nullptr), _param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false), _param_yaw_timeout(this, "MIS_YAW_TMT", false), _param_yaw_err(this, "MIS_YAW_ERR", false), @@ -86,10 +69,6 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : { } -MissionBlock::~MissionBlock() -{ -} - bool MissionBlock::is_mission_item_reached() { @@ -125,6 +104,7 @@ MissionBlock::is_mission_item_reached() * We wait half a second to give the transition command time to propagate. * Then monitor the transition status for completion. */ + // TODO: check desired transition state achieved and drop _action_start if (hrt_absolute_time() - _action_start > 500000 && !_navigator->get_vstatus()->in_transition_mode) { @@ -145,8 +125,7 @@ MissionBlock::is_mission_item_reached() hrt_abstime now = hrt_absolute_time(); - if ((_navigator->get_land_detected()->landed == false) - && !_waypoint_position_reached) { + if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) { float dist = -1.0f; float dist_xy = -1.0f; @@ -375,8 +354,8 @@ MissionBlock::is_mission_item_reached() } /* check if the MAV was long enough inside the waypoint orbit */ - if ((Navigator::get_time_inside(_mission_item) < FLT_EPSILON) || - (now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) { + if ((get_time_inside(_mission_item) < FLT_EPSILON) || + (now - _time_first_inside_orbit >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) { // exit xtrack location if (_mission_item.loiter_exit_xtrack && @@ -459,17 +438,17 @@ MissionBlock::issue_command(const struct mission_item_s *item) if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) { PX4_INFO("do_set_servo command"); // XXX: we should issue a vehicle command and handle this somewhere else - memset(&actuators, 0, sizeof(actuators)); + _actuators = {}; // params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6) // params[1] new value for selected actuator in ms 900...2000 - actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1]; - actuators.timestamp = hrt_absolute_time(); + _actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1]; + _actuators.timestamp = hrt_absolute_time(); if (_actuator_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators); + orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); } else { - _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators); + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); } } else { @@ -478,15 +457,20 @@ MissionBlock::issue_command(const struct mission_item_s *item) mission_item_to_vehicle_command(item, &cmd); _action_start = hrt_absolute_time(); - if (_cmd_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); - - } else { - _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH); - } + _navigator->publish_vehicle_cmd(cmd); } } +float +MissionBlock::get_time_inside(const struct mission_item_s &item) +{ + if (item.nav_cmd == NAV_CMD_TAKEOFF) { + return item.time_inside; + } + + return 0.0f; +} + bool MissionBlock::item_contains_position(const struct mission_item_s *item) { @@ -663,7 +647,6 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION; /* use current target position */ - item->lat = target.lat; item->lon = target.lon; item->altitude = _navigator->get_home_position()->alt; @@ -709,18 +692,15 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio { /* VTOL transition to RW before landing */ - if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing && + if (_navigator->get_vstatus()->is_vtol && + !_navigator->get_vstatus()->is_rotary_wing && _param_force_vtol.get() == 1) { + struct vehicle_command_s cmd = {}; cmd.command = NAV_CMD_DO_VTOL_TRANSITION; cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - if (_cmd_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); - - } else { - _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH); - } + _navigator->publish_vehicle_cmd(cmd); } /* set the land item */ @@ -732,9 +712,8 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio item->lon = _navigator->get_global_position()->lon; item->yaw = _navigator->get_local_position()->yaw; - /* use home position */ - } else { + /* use home position */ item->lat = _navigator->get_home_position()->lat; item->lon = _navigator->get_home_position()->lon; item->yaw = _navigator->get_home_position()->yaw; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 4676cf3846..134b9fa443 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -41,18 +41,16 @@ #ifndef NAVIGATOR_MISSION_BLOCK_H #define NAVIGATOR_MISSION_BLOCK_H -#include +#include "navigator_mode.h" #include - -#include -#include -#include -#include #include #include - -#include "navigator_mode.h" +#include +#include +#include +#include +#include class Navigator; @@ -63,15 +61,11 @@ public: * Constructor */ MissionBlock(Navigator *navigator, const char *name); + ~MissionBlock() = default; MissionBlock(const MissionBlock &) = delete; MissionBlock &operator=(const MissionBlock &) = delete; - /** - * Destructor - */ - virtual ~MissionBlock(); - /* TODO: move this to a helper class in navigator */ static bool item_contains_position(const struct mission_item_s *item); @@ -81,6 +75,7 @@ protected: * @return true if successfully reached */ bool is_mission_item_reached(); + /** * Reset all reached flags */ @@ -121,28 +116,31 @@ protected: */ void set_idle_item(struct mission_item_s *item); - /** - * Convert a mission item to a command - */ - void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd); - /** * Set follow_target item */ void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw); + /** + * Convert a mission item to a command + */ + void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd); + void issue_command(const struct mission_item_s *item); - mission_item_s _mission_item; - bool _waypoint_position_reached; - bool _waypoint_yaw_reached; - hrt_abstime _time_first_inside_orbit; - hrt_abstime _action_start; - hrt_abstime _time_wp_reached; + float get_time_inside(const struct mission_item_s &item); - actuator_controls_s _actuators; - orb_advert_t _actuator_pub; - orb_advert_t _cmd_pub; + mission_item_s _mission_item{}; + + bool _waypoint_position_reached{false}; + bool _waypoint_yaw_reached{false}; + + hrt_abstime _time_first_inside_orbit{0}; + hrt_abstime _action_start{0}; + hrt_abstime _time_wp_reached{0}; + + actuator_controls_s _actuators{}; + orb_advert_t _actuator_pub{nullptr}; control::BlockParamFloat _param_loiter_min_alt; control::BlockParamFloat _param_yaw_timeout; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index e192e54108..3f619c6879 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -1,6 +1,6 @@ /*************************************************************************** * - * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,37 +41,34 @@ #ifndef NAVIGATOR_H #define NAVIGATOR_H -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "navigator_mode.h" -#include "mission.h" -#include "loiter.h" -#include "takeoff.h" -#include "land.h" -#include "rtl.h" #include "datalinkloss.h" #include "enginefailure.h" #include "follow_target.h" -#include "gpsfailure.h" -#include "rcloss.h" #include "geofence.h" +#include "gpsfailure.h" +#include "land.h" +#include "loiter.h" +#include "mission.h" +#include "navigator_mode.h" +#include "rcloss.h" +#include "rtl.h" +#include "takeoff.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include /** * Number of navigation modes that need on_active/on_inactive calls @@ -81,15 +78,10 @@ class Navigator : public control::SuperBlock { public: - /** - * Constructor - */ Navigator(); - - /** - * Destructor, also kills the navigators task. - */ ~Navigator(); + Navigator(const Navigator &) = delete; + Navigator operator=(const Navigator &) = delete; /** * Start the navigator task. @@ -124,6 +116,8 @@ public: */ void publish_att_sp(); + void publish_vehicle_cmd(const struct vehicle_command_s &vcmd); + /** * Setters */ @@ -134,21 +128,20 @@ public: /** * Getters */ - struct vehicle_status_s *get_vstatus() { return &_vstatus; } - struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; } - struct vehicle_control_mode_s *get_control_mode() { return &_control_mode; } - struct vehicle_global_position_s *get_global_position() { return &_global_pos; } - struct vehicle_local_position_s *get_local_position() { return &_local_pos; } - struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; } - struct sensor_combined_s *get_sensor_combined() { return &_sensor_combined; } + struct fw_pos_ctrl_status_s *get_fw_pos_ctrl_status() { return &_fw_pos_ctrl_status; } struct home_position_s *get_home_position() { return &_home_pos; } - bool home_position_valid() { return (_home_pos.timestamp > 0); } + 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 mission_result_s *get_mission_result() { return &_mission_result; } - struct geofence_result_s *get_geofence_result() { return &_geofence_result; } struct vehicle_attitude_setpoint_s *get_att_sp() { return &_att_sp; } + struct vehicle_global_position_s *get_global_position() { return &_global_pos; } + struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; } + 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; } + + bool home_position_valid() { return (_home_pos.timestamp > 0); } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } @@ -188,7 +181,7 @@ public: * Passing a negative value or leaving the parameter away will reset the cruising speed * to its default value. * - * For VTOL: sets cuising speed for current mode only (multirotor or fixed-wing). + * For VTOL: sets cruising speed for current mode only (multirotor or fixed-wing). * */ void set_cruising_speed(float speed = -1.0f); @@ -230,83 +223,70 @@ public: bool abort_landing(); - static float get_time_inside(struct mission_item_s &item) { return (item.nav_cmd == NAV_CMD_TAKEOFF) ? 0.0f : item.time_inside; } - private: - bool _task_should_exit; /**< if true, sensor task should exit */ - int _navigator_task; /**< task handle for sensor task */ + bool _task_should_exit{false}; /**< if true, sensor task should exit */ + int _navigator_task{-1}; /**< task handle for sensor task */ - orb_advert_t _mavlink_log_pub; /**< the uORB advert to send messages over mavlink */ + orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */ - int _global_pos_sub; /**< global position subscription */ - int _local_pos_sub; /**< local position subscription */ - int _gps_pos_sub; /**< gps position subscription */ - int _sensor_combined_sub; /**< sensor combined subscription */ - int _home_pos_sub; /**< home position subscription */ - int _vstatus_sub; /**< vehicle status subscription */ - int _land_detected_sub; /**< vehicle land detected subscription */ - int _fw_pos_ctrl_status_sub; /**< notification of vehicle capabilities updates */ - int _control_mode_sub; /**< vehicle control mode subscription */ - int _onboard_mission_sub; /**< onboard mission subscription */ - int _offboard_mission_sub; /**< offboard mission subscription */ - int _param_update_sub; /**< param update subscription */ - int _vehicle_command_sub; /**< vehicle commands (onboard and offboard) */ + int _fw_pos_ctrl_status_sub{-1}; /**< notification of vehicle capabilities updates */ + int _global_pos_sub{-1}; /**< global position subscription */ + int _gps_pos_sub{-1}; /**< gps position subscription */ + int _home_pos_sub{-1}; /**< home position subscription */ + int _land_detected_sub{-1}; /**< vehicle land detected subscription */ + int _local_pos_sub{-1}; /**< local position subscription */ + int _offboard_mission_sub{-1}; /**< offboard mission subscription */ + int _onboard_mission_sub{-1}; /**< onboard mission subscription */ + int _param_update_sub{-1}; /**< param update subscription */ + 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 */ - orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ - orb_advert_t _mission_result_pub; - orb_advert_t _geofence_result_pub; - orb_advert_t _att_sp_pub; /**< publish att sp - used only in very special failsafe modes - when pos control is deactivated */ + orb_advert_t _att_sp_pub{nullptr}; + orb_advert_t _geofence_result_pub{nullptr}; + orb_advert_t _mission_result_pub{nullptr}; + orb_advert_t _pos_sp_triplet_pub{nullptr}; + orb_advert_t _vehicle_cmd_pub{nullptr}; - vehicle_status_s _vstatus; /**< vehicle status */ - vehicle_land_detected_s _land_detected; /**< vehicle land_detected */ - vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - vehicle_global_position_s _global_pos; /**< global vehicle position */ + fw_pos_ctrl_status_s _fw_pos_ctrl_status{}; /**< fixed wing navigation capabilities */ + geofence_result_s _geofence_result{}; + home_position_s _home_pos{}; /**< home position for RTL */ + mission_result_s _mission_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 */ + sensor_combined_s _sensor_combined{}; /**< sensor values */ + vehicle_attitude_setpoint_s _att_sp{}; + vehicle_global_position_s _global_pos{}; /**< global vehicle position */ + vehicle_gps_position_s _gps_pos{}; /**< gps position */ + vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */ vehicle_local_position_s _local_pos; /**< local vehicle position */ - vehicle_gps_position_s _gps_pos; /**< gps position */ - sensor_combined_s _sensor_combined; /**< sensor values */ - home_position_s _home_pos; /**< home position for RTL */ - mission_item_s _mission_item; /**< current mission item */ - fw_pos_ctrl_status_s _fw_pos_ctrl_status; /**< fixed wing navigation capabilities */ - 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_status_s _vstatus{}; /**< vehicle status */ - mission_result_s _mission_result; - geofence_result_s _geofence_result; - vehicle_attitude_setpoint_s _att_sp; - - bool _mission_item_valid; /**< flags if the current mission item is valid */ - int _mission_instance_count; /**< instance count for the current mission */ + int _mission_instance_count{-1}; /**< instance count for the current mission */ perf_counter_t _loop_perf; /**< loop performance counter */ Geofence _geofence; /**< class that handles the geofence */ - bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ + bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */ - bool _inside_fence; /**< vehicle is inside fence */ + 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 */ - bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ - bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ - bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ - bool _mission_result_updated; /**< flags if mission result has seen an update */ - - NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ + NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */ Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ Takeoff _takeoff; /**< class for handling takeoff commands */ Land _land; /**< class for handling land commands */ RTL _rtl; /**< class that handles RTL */ - RCLoss _rcLoss; /**< class that handles RTL according to - OBC rules (rc loss mode) */ + RCLoss _rcLoss; /**< class that handles RTL according to OBC rules (rc loss mode) */ DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ - EngineFailure _engineFailure; /**< class that handles the engine failure mode - (FW only!) */ + EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */ GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */ - - FollowTarget _follow_target; + FollowTarget _follow_target; NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ @@ -319,59 +299,20 @@ private: control::BlockParamFloat _param_cruising_speed_plane; control::BlockParamFloat _param_cruising_throttle_plane; - float _mission_cruising_speed_mc; - float _mission_cruising_speed_fw; - float _mission_throttle; + float _mission_cruising_speed_mc{-1.0f}; + float _mission_cruising_speed_fw{-1.0f}; + float _mission_throttle{-1.0f}; - /** - * Retrieve global position - */ - void global_position_update(); - - /** - * Retrieve local position - */ - void local_position_update(); - - /** - * Retrieve gps position - */ - void gps_position_update(); - - /** - * Retrieve sensor values - */ - void sensor_combined_update(); - - /** - * Retrieve home position - */ - void home_position_update(bool force = false); - - /** - * Retrieve fixed wing navigation capabilities - */ + // update subscriptions void fw_pos_ctrl_status_update(); - - /** - * Retrieve vehicle status - */ - void vehicle_status_update(); - - /** - * Retrieve vehicle land detected - */ - void vehicle_land_detected_update(); - - /** - * Retrieve vehicle control mode - */ - void vehicle_control_mode_update(); - - /** - * Update parameters - */ + void global_position_update(); + void gps_position_update(); + void home_position_update(bool force = false); + void local_position_update(); void params_update(); + void sensor_combined_update(); + void vehicle_land_detected_update(); + void vehicle_status_update(); /** * Shim for calling task_main from task_create. @@ -393,16 +334,9 @@ private: */ void publish_position_setpoint_triplet(); - /** * Publish the mission result so commander and mavlink know what is going on */ void publish_mission_result(); - - /* this class has ptr data members, so it should not be copied, - * consequently the copy constructors are private. - */ - Navigator(const Navigator &); - Navigator operator=(const Navigator &); }; #endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 644c2c1d69..a10121686b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -47,9 +47,7 @@ #include -#include #include -#include #include #include #include @@ -60,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -82,57 +79,13 @@ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); namespace navigator { - Navigator *g_navigator; } Navigator::Navigator() : SuperBlock(nullptr, "NAV"), - _task_should_exit(false), - _navigator_task(-1), - _mavlink_log_pub(nullptr), - _global_pos_sub(-1), - _local_pos_sub(-1), - _gps_pos_sub(-1), - _sensor_combined_sub(-1), - _home_pos_sub(-1), - _vstatus_sub(-1), - _land_detected_sub(-1), - _fw_pos_ctrl_status_sub(-1), - _control_mode_sub(-1), - _onboard_mission_sub(-1), - _offboard_mission_sub(-1), - _param_update_sub(-1), - _vehicle_command_sub(-1), - _pos_sp_triplet_pub(nullptr), - _mission_result_pub(nullptr), - _geofence_result_pub(nullptr), - _att_sp_pub(nullptr), - _vstatus{}, - _land_detected{}, - _control_mode{}, - _global_pos{}, - _gps_pos{}, - _sensor_combined{}, - _home_pos{}, - _mission_item{}, - _fw_pos_ctrl_status{}, - _pos_sp_triplet{}, - _reposition_triplet{}, - _takeoff_triplet{}, - _mission_result{}, - _att_sp{}, - _mission_item_valid(false), - _mission_instance_count(0), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence(this), - _geofence_violation_warning_sent(false), - _inside_fence(true), - _can_loiter_at_sp(false), - _pos_sp_triplet_updated(false), - _pos_sp_triplet_published_invalid_once(false), - _mission_result_updated(false), - _navigation_mode(nullptr), _mission(this, "MIS"), _loiter(this, "LOI"), _takeoff(this, "TKF"), @@ -149,10 +102,7 @@ Navigator::Navigator() : _param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"), _param_cruising_speed_hover(this, "MPC_XY_CRUISE", false), _param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false), - _param_cruising_throttle_plane(this, "FW_THR_CRUISE", false), - _mission_cruising_speed_mc(-1.0f), - _mission_cruising_speed_fw(-1.0f), - _mission_throttle(-1.0f) + _param_cruising_throttle_plane(this, "FW_THR_CRUISE", false) { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; @@ -232,7 +182,12 @@ Navigator::home_position_update(bool force) void Navigator::fw_pos_ctrl_status_update() { - orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status); + bool updated = false; + orb_check(_fw_pos_ctrl_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status); + } } void @@ -250,16 +205,6 @@ Navigator::vehicle_land_detected_update() orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected); } -void -Navigator::vehicle_control_mode_update() -{ - if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) { - /* in case the commander is not be running */ - _control_mode.flag_control_auto_enabled = false; - _control_mode.flag_armed = false; - } -} - void Navigator::params_update() { @@ -306,7 +251,6 @@ Navigator::task_main() _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); @@ -316,7 +260,6 @@ Navigator::task_main() /* copy all topics first time */ vehicle_status_update(); vehicle_land_detected_update(); - vehicle_control_mode_update(); global_position_update(); local_position_update(); gps_position_update(); @@ -407,13 +350,6 @@ Navigator::task_main() params_update(); } - /* vehicle control mode updated */ - orb_check(_control_mode_sub, &updated); - - if (updated) { - vehicle_control_mode_update(); - } - /* vehicle status updated */ orb_check(_vstatus_sub, &updated); @@ -445,7 +381,7 @@ Navigator::task_main() orb_check(_vehicle_command_sub, &updated); if (updated) { - vehicle_command_s cmd; + vehicle_command_s cmd = {}; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { @@ -532,7 +468,7 @@ Navigator::task_main() /* find NAV_CMD_DO_LAND_START in the mission and * use MAV_CMD_MISSION_START to start the mission there */ - unsigned land_start = _mission.find_offboard_land_start(); + int land_start = _mission.find_offboard_land_start(); if (land_start != -1) { vehicle_command_s vcmd = {}; @@ -542,8 +478,10 @@ Navigator::task_main() vcmd.param1 = land_start; vcmd.param2 = 0; - orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); - (void)orb_unadvertise(pub); + publish_vehicle_cmd(vcmd); + + } else { + PX4_WARN("planned landing not available"); } } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH) { @@ -598,12 +536,12 @@ Navigator::task_main() last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; + _geofence_result.timestamp = hrt_absolute_time(); _geofence_result.geofence_action = _geofence.getGeofenceAction(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; - publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { @@ -614,10 +552,12 @@ Navigator::task_main() } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; - publish_geofence_result(); + /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } + + publish_geofence_result(); } /* Do stuff according to navigation state set by commander */ @@ -748,24 +688,8 @@ Navigator::start() void Navigator::status() { - /* TODO: add this again */ - // PX4_INFO("Global position is %svalid", _global_pos_valid ? "" : "in"); - - // if (_global_pos.global_valid) { - // PX4_INFO("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); - // PX4_INFO("Altitude %5.5f meters, altitude above home %5.5f meters", - // (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); - // PX4_INFO("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", - // (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); - // PX4_INFO("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); - // } - if (_geofence.valid()) { PX4_INFO("Geofence is valid"); - /* TODO: needed? */ -// PX4_INFO("Vertex longitude latitude"); -// for (unsigned i = 0; i < _fence.count; i++) -// PX4_INFO("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); } else { PX4_INFO("Geofence not set (no /etc/geofence.txt on microsd) or not valid"); @@ -804,7 +728,7 @@ Navigator::get_acceptance_radius() float Navigator::get_altitude_acceptance_radius() { - if (!this->get_vstatus()->is_rotary_wing) { + if (!get_vstatus()->is_rotary_wing) { return _param_fw_alt_acceptance_radius.get(); } else { @@ -817,8 +741,7 @@ Navigator::get_cruising_speed() { /* there are three options: The mission-requested cruise speed, or the current hover / plane speed */ if (_vstatus.is_rotary_wing) { - if (_mission_cruising_speed_mc > 0.0f - && _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { + if (is_planned_mission() && _mission_cruising_speed_mc > 0.0f) { return _mission_cruising_speed_mc; } else { @@ -826,8 +749,7 @@ Navigator::get_cruising_speed() } } else { - if (_mission_cruising_speed_fw > 0.0f - && _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { + if (is_planned_mission() && _mission_cruising_speed_fw > 0.0f) { return _mission_cruising_speed_fw; } else { @@ -1008,7 +930,6 @@ Navigator::publish_mission_result() void Navigator::publish_geofence_result() { - /* lazily publish the geofence result only once available */ if (_geofence_result_pub != nullptr) { /* publish mission result */ @@ -1034,6 +955,17 @@ Navigator::publish_att_sp() } } +void +Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd) +{ + if (_vehicle_cmd_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vcmd); + + } else { + _vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + } +} + void Navigator::set_mission_failure(const char *reason) { diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index f7ccda0cbd..9d1fad72ae 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -53,10 +53,6 @@ NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : on_inactive(); } -NavigatorMode::~NavigatorMode() -{ -} - void NavigatorMode::run(bool active) { diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 18cfd26db9..fff39ac2f0 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -56,15 +56,10 @@ class Navigator; class NavigatorMode : public control::SuperBlock { public: - /** - * Constructor - */ NavigatorMode(Navigator *navigator, const char *name); - - /** - * Destructor - */ - virtual ~NavigatorMode(); + virtual ~NavigatorMode() = default; + NavigatorMode(const NavigatorMode &) = delete; + NavigatorMode operator=(const NavigatorMode &) = delete; void run(bool active); @@ -84,16 +79,10 @@ public: virtual void on_active(); protected: - Navigator *_navigator; + Navigator *_navigator{nullptr}; private: - bool _first_run; - - /* this class has ptr data members, so it should not be copied, - * consequently the copy constructors are private. - */ - NavigatorMode(const NavigatorMode &); - NavigatorMode operator=(const NavigatorMode &); + bool _first_run{true}; }; #endif diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 25f22e11f2..e9418335eb 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -268,9 +268,9 @@ RTL::set_rtl_item() _navigator->set_can_loiter_at_sp(true); - if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) { + if (autoland && (get_time_inside(_mission_item) > FLT_EPSILON)) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", - (double)Navigator::get_time_inside(_mission_item)); + (double)get_time_inside(_mission_item)); } else { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");