Standardize method name style in navigator_main.cpp and whitespace formatting in navigator.h

This commit is contained in:
mcsauder
2022-01-29 21:36:11 -05:00
committed by Daniel Agar
parent 3862a24b8e
commit 463ac8e8a1
2 changed files with 160 additions and 183 deletions
+120 -112
View File
@@ -117,9 +117,9 @@ public:
/** /**
* Load fence from file * Load fence from file
*/ */
void load_fence_from_file(const char *filename); void load_fence_from_file(const char *filename);
void publish_vehicle_cmd(vehicle_command_s *vcmd); void publish_vehicle_cmd(vehicle_command_s *vcmd);
/** /**
* Generate an artificial traffic indication * Generate an artificial traffic indication
@@ -132,83 +132,87 @@ public:
* @param ver_velocity Vertical velocity of traffic, in m/s * @param ver_velocity Vertical velocity of traffic, in m/s
* @param emitter_type, Type of vehicle, as a number * @param emitter_type, Type of vehicle, as a number
*/ */
void fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, float altitude_diff, void fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, float altitude_diff,
float hor_velocity, float ver_velocity, int emitter_type); float hor_velocity, float ver_velocity, int emitter_type);
/** /**
* Check nearby traffic for potential collisions * Check nearby traffic for potential collisions
*/ */
void check_traffic(); void check_traffic();
/** /**
* Buffer for air traffic to control the amount of messages sent to a user * Buffer for air traffic to control the amount of messages sent to a user
*/ */
bool buffer_air_traffic(uint32_t icao_address); bool buffer_air_traffic(uint32_t icao_address);
/** /**
* Setters * Setters
*/ */
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } 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; } void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
void set_mission_result_updated() { _mission_result_updated = true; } void set_mission_result_updated() { _mission_result_updated = true; }
/** /**
* Getters * Getters
*/ */
struct home_position_s *get_home_position() { return &_home_pos; } home_position_s *get_home_position() { return &_home_pos; }
struct mission_result_s *get_mission_result() { return &_mission_result; } mission_result_s *get_mission_result() { return &_mission_result; }
struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; } position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; } position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; }
struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; } position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; }
struct vehicle_global_position_s *get_global_position() { return &_global_pos; } vehicle_global_position_s *get_global_position() { return &_global_pos; }
struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; } vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
struct vehicle_local_position_s *get_local_position() { return &_local_pos; } vehicle_local_position_s *get_local_position() { return &_local_pos; }
struct vehicle_status_s *get_vstatus() { return &_vstatus; } vehicle_status_s *get_vstatus() { return &_vstatus; }
PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */ PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */
const vehicle_roi_s &get_vroi() { return _vroi; } const vehicle_roi_s &get_vroi() { return _vroi; }
void reset_vroi() { _vroi = {}; } void reset_vroi() { _vroi = {}; }
bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); } bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); }
bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos && _home_pos.valid_lpos); } bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos && _home_pos.valid_lpos); }
Geofence &get_geofence() { return _geofence; } Geofence &get_geofence() { return _geofence; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_nav_loiter_rad.get(); }
float get_loiter_radius() { return _param_nav_loiter_rad.get(); }
/** /**
* Returns the default acceptance radius defined by the parameter * Returns the default acceptance radius defined by the parameter
*/ */
float get_default_acceptance_radius(); float get_default_acceptance_radius();
/** /**
* Get the acceptance radius * Get the acceptance radius
* *
* @return the distance at which the next waypoint should be used * @return the distance at which the next waypoint should be used
*/ */
float get_acceptance_radius(); float get_acceptance_radius();
/** /**
* Get the default altitude acceptance radius (i.e. from parameters) * Get the default altitude acceptance radius (i.e. from parameters)
* *
* @return the distance from the target altitude before considering the waypoint reached * @return the distance from the target altitude before considering the waypoint reached
*/ */
float get_default_altitude_acceptance_radius(); float get_default_altitude_acceptance_radius();
/** /**
* Get the altitude acceptance radius * Get the altitude acceptance radius
* *
* @return the distance from the target altitude before considering the waypoint reached * @return the distance from the target altitude before considering the waypoint reached
*/ */
float get_altitude_acceptance_radius(); float get_altitude_acceptance_radius();
/** /**
* Get the cruising speed * Get the cruising speed
* *
* @return the desired cruising speed for this mission * @return the desired cruising speed for this mission
*/ */
float get_cruising_speed(); float get_cruising_speed();
/** /**
* Set the cruising speed * Set the cruising speed
@@ -219,36 +223,36 @@ public:
* For VTOL: sets cruising 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); void set_cruising_speed(float speed = -1.0f);
/** /**
* Reset cruising speed to default values * Reset cruising speed to default values
* *
* For VTOL: resets both cruising speeds. * For VTOL: resets both cruising speeds.
*/ */
void reset_cruising_speed(); void reset_cruising_speed();
/** /**
* Set triplets to invalid * Set triplets to invalid
*/ */
void reset_triplets(); void reset_triplets();
/** /**
* Set position setpoint to safe defaults * Set position setpoint to safe defaults
*/ */
void reset_position_setpoint(position_setpoint_s &sp); void reset_position_setpoint(position_setpoint_s &sp);
/** /**
* Get the target throttle * Get the target throttle
* *
* @return the desired throttle for this mission * @return the desired throttle for this mission
*/ */
float get_cruising_throttle(); float get_cruising_throttle();
/** /**
* Set the target throttle * Set the target throttle
*/ */
void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; } void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; }
/** /**
* Get the yaw acceptance given the current mission item * Get the yaw acceptance given the current mission item
@@ -258,92 +262,75 @@ public:
* @return the yaw at which the next waypoint should be used or NaN if the yaw at a waypoint * @return the yaw at which the next waypoint should be used or NaN if the yaw at a waypoint
* should be ignored * should be ignored
*/ */
float get_yaw_acceptance(float mission_item_yaw); float get_yaw_acceptance(float mission_item_yaw);
orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }
orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } void increment_mission_instance_count() { _mission_result.instance_count++; }
void increment_mission_instance_count() { _mission_result.instance_count++; } int mission_instance_count() const { return _mission_result.instance_count; }
int mission_instance_count() const { return _mission_result.instance_count; }
void set_mission_failure_heading_timeout(); void set_mission_failure_heading_timeout();
void setMissionLandingInProgress(bool in_progress) { _mission_landing_in_progress = in_progress; } void setMissionLandingInProgress(bool in_progress) { _mission_landing_in_progress = in_progress; }
bool getMissionLandingInProgress() { return _mission_landing_in_progress; } bool getMissionLandingInProgress() { return _mission_landing_in_progress; }
bool is_planned_mission() const { return _navigation_mode == &_mission; } bool is_planned_mission() const { return _navigation_mode == &_mission; }
bool on_mission_landing() { return _mission.landing(); }
bool start_mission_landing() { return _mission.land_start(); }
bool get_mission_start_land_available() { return _mission.get_land_start_available(); }
int get_mission_landing_index() { return _mission.get_land_start_index(); }
double get_mission_landing_start_lat() { return _mission.get_landing_start_lat(); }
double get_mission_landing_start_lon() { return _mission.get_landing_start_lon(); }
float get_mission_landing_start_alt() { return _mission.get_landing_start_alt(); }
double get_mission_landing_lat() { return _mission.get_landing_lat(); } bool on_mission_landing() { return _mission.landing(); }
double get_mission_landing_lon() { return _mission.get_landing_lon(); }
float get_mission_landing_alt() { return _mission.get_landing_alt(); } bool start_mission_landing() { return _mission.land_start(); }
bool get_mission_start_land_available() { return _mission.get_land_start_available(); }
int get_mission_landing_index() { return _mission.get_land_start_index(); }
double get_mission_landing_start_lat() { return _mission.get_landing_start_lat(); }
double get_mission_landing_start_lon() { return _mission.get_landing_start_lon(); }
float get_mission_landing_start_alt() { return _mission.get_landing_start_alt(); }
double get_mission_landing_lat() { return _mission.get_landing_lat(); }
double get_mission_landing_lon() { return _mission.get_landing_lon(); }
float get_mission_landing_alt() { return _mission.get_landing_alt(); }
// RTL // RTL
bool mission_landing_required() { return _rtl.get_rtl_type() == RTL::RTL_TYPE_MISSION_LANDING; } bool mission_landing_required() { return _rtl.get_rtl_type() == RTL::RTL_TYPE_MISSION_LANDING; }
bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }
bool abort_landing(); bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }
bool abort_landing();
void geofence_breach_check(bool &have_geofence_position_data); void geofence_breach_check(bool &have_geofence_position_data);
// Param access // Param access
float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); } float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); }
float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); } float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); }
bool get_takeoff_required() const { return _param_mis_takeoff_req.get(); } bool get_takeoff_required() const { return _param_mis_takeoff_req.get(); }
float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); } float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); }
float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); } float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); }
float get_lndmc_alt_max() const { return _param_lndmc_alt_max.get(); } float get_lndmc_alt_max() const { return _param_lndmc_alt_max.get(); }
float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; } float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; }
float get_vtol_reverse_delay() const { return _param_reverse_delay; } float get_vtol_reverse_delay() const { return _param_reverse_delay; }
bool force_vtol(); bool force_vtol();
void acquire_gimbal_control(); void acquire_gimbal_control();
void release_gimbal_control(); void release_gimbal_control();
private: private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad, /**< loiter radius for fixedwing */
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad, /**< acceptance for takeoff */
(ParamFloat<px4::params::NAV_FW_ALT_RAD>)
_param_nav_fw_alt_rad, /**< acceptance radius for fixedwing altitude */
(ParamFloat<px4::params::NAV_FW_ALTL_RAD>)
_param_nav_fw_altl_rad, /**< acceptance radius for fixedwing altitude before landing*/
(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
_param_nav_mc_alt_rad, /**< acceptance radius for multicopter altitude */
(ParamInt<px4::params::NAV_FORCE_VT>) _param_nav_force_vt, /**< acceptance radius for multicopter altitude */
(ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */
(ParamFloat<px4::params::NAV_TRAFF_A_RADU>) _param_nav_traff_a_radu, /**< avoidance Distance Unmanned*/
(ParamFloat<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/
// non-navigator parameters
// Mission (MIS_*)
(ParamFloat<px4::params::MIS_LTRMIN_ALT>) _param_mis_ltrmin_alt,
(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
(ParamBool<px4::params::MIS_TAKEOFF_REQ>) _param_mis_takeoff_req,
(ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err,
(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max
)
struct traffic_buffer_s { struct traffic_buffer_s {
uint32_t icao_address; uint32_t icao_address;
hrt_abstime timestamp; hrt_abstime timestamp;
}; };
int _local_pos_sub{-1}; int _local_pos_sub{-1};
int _mission_sub{-1}; int _mission_sub{-1};
int _vehicle_status_sub{-1}; int _vehicle_status_sub{-1};
uORB::SubscriptionData<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -355,18 +342,15 @@ private:
uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */ uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */ uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */
uORB::SubscriptionData<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
uORB::Publication<geofence_result_s> _geofence_result_pub{ORB_ID(geofence_result)}; uORB::Publication<geofence_result_s> _geofence_result_pub{ORB_ID(geofence_result)};
uORB::Publication<mission_result_s> _mission_result_pub{ORB_ID(mission_result)}; uORB::Publication<mission_result_s> _mission_result_pub{ORB_ID(mission_result)};
uORB::Publication<position_setpoint_triplet_s> _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; uORB::Publication<position_setpoint_triplet_s> _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
uORB::Publication<vehicle_command_ack_s> _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_command_s> _vehicle_cmd_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_roi_s> _vehicle_roi_pub{ORB_ID(vehicle_roi)}; uORB::Publication<vehicle_roi_s> _vehicle_roi_pub{ORB_ID(vehicle_roi)};
orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */ orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */
uORB::Publication<vehicle_command_ack_s> _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_command_s> _vehicle_cmd_pub{ORB_ID(vehicle_command)};
// Subscriptions // Subscriptions
home_position_s _home_pos{}; /**< home position for RTL */ home_position_s _home_pos{}; /**< home position for RTL */
mission_result_s _mission_result{}; mission_result_s _mission_result{};
@@ -388,26 +372,28 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _loop_perf; /**< loop performance counter */
Geofence _geofence; /**< class that handles the geofence */ Geofence _geofence; /**< class that handles the geofence */
bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */
GeofenceBreachAvoidance _gf_breach_avoidance; GeofenceBreachAvoidance _gf_breach_avoidance;
hrt_abstime _last_geofence_check = 0; hrt_abstime _last_geofence_check = 0;
bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */
bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */ 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_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 _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 _mission_result_updated{false}; /**< flags if mission result has seen an update */
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
Mission _mission; /**< class that handles the missions */ Mission _mission; /**< class that handles the missions */
Loiter _loiter; /**< class that handles loiter */ Loiter _loiter; /**< class that handles loiter */
Takeoff _takeoff; /**< class for handling takeoff commands */ Takeoff _takeoff; /**< class for handling takeoff commands */
Land _land; /**< class for handling land commands */ Land _land; /**< class for handling land commands */
PrecLand _precland; /**< class for handling precision land commands */ PrecLand _precland; /**< class for handling precision land commands */
RTL _rtl; /**< class that handles RTL */ RTL _rtl; /**< class that handles RTL */
EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */ EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */
FollowTarget _follow_target; FollowTarget _follow_target;
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE] {}; /**< array of navigation modes */
param_t _handle_back_trans_dec_mss{PARAM_INVALID}; param_t _handle_back_trans_dec_mss{PARAM_INVALID};
param_t _handle_reverse_delay{PARAM_INVALID}; param_t _handle_reverse_delay{PARAM_INVALID};
@@ -416,32 +402,54 @@ private:
float _param_back_trans_dec_mss{0.f}; float _param_back_trans_dec_mss{0.f};
float _param_reverse_delay{0.f}; float _param_reverse_delay{0.f};
float _param_mpc_jerk_auto{4.f}; /**< initialized with the default jerk auto value to prevent division by 0 if the parameter is accidentally set to 0 */ float _param_mpc_jerk_auto{4.f}; /**< initialized with the default jerk auto value to prevent division by 0 if the parameter is accidentally set to 0 */
float _param_mpc_acc_hor{3.f}; /**< initialized with the default horizontal acc value to prevent division by 0 if the parameter is accidentally set to 0 */ float _param_mpc_acc_hor{3.f}; /**< initialized with the default horizontal acc value to prevent division by 0 if the parameter is accidentally set to 0 */
float _mission_cruising_speed_mc{-1.0f}; float _mission_cruising_speed_mc{-1.0f};
float _mission_cruising_speed_fw{-1.0f}; float _mission_cruising_speed_fw{-1.0f};
float _mission_throttle{NAN}; float _mission_throttle{NAN};
bool _mission_landing_in_progress{false}; // this flag gets set if the mission is currently executing on a landing pattern bool _mission_landing_in_progress{false}; /**< this flag gets set if the mission is currently executing on a landing pattern
// if mission mode is inactive, this flag will be cleared after 2 seconds * if mission mode is inactive, this flag will be cleared after 2 seconds */
traffic_buffer_s _traffic_buffer{}; traffic_buffer_s _traffic_buffer{};
// update subscriptions // update subscriptions
void params_update(); void params_update();
/** /**
* Publish a new position setpoint triplet for position controllers * Publish a new position setpoint triplet for position controllers
*/ */
void publish_position_setpoint_triplet(); void publish_position_setpoint_triplet();
/** /**
* Publish the mission result so commander and mavlink know what is going on * Publish the mission result so commander and mavlink know what is going on
*/ */
void publish_mission_result(); void publish_mission_result();
void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result); void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
bool geofence_allows_position(const vehicle_global_position_s &pos); bool geofence_allows_position(const vehicle_global_position_s &pos);
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad, /**< loiter radius for fixedwing */
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad, /**< acceptance for takeoff */
(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad, /**< acceptance rad for fixedwing alt */
(ParamFloat<px4::params::NAV_FW_ALTL_RAD>)
_param_nav_fw_altl_rad, /**< acceptance rad for fixedwing alt before landing*/
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad, /**< acceptance rad for multicopter alt */
(ParamInt<px4::params::NAV_FORCE_VT>) _param_nav_force_vt, /**< acceptance radius for multicopter alt */
(ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */
(ParamFloat<px4::params::NAV_TRAFF_A_RADU>) _param_nav_traff_a_radu, /**< avoidance Distance Unmanned*/
(ParamFloat<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/
// non-navigator parameters
// Mission (MIS_*)
(ParamFloat<px4::params::MIS_LTRMIN_ALT>) _param_mis_ltrmin_alt,
(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
(ParamBool<px4::params::MIS_TAKEOFF_REQ>) _param_mis_takeoff_req,
(ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err,
(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max
)
}; };
+40 -71
View File
@@ -59,16 +59,11 @@
#include <px4_platform_common/tasks.h> #include <px4_platform_common/tasks.h>
#include <systemlib/mavlink_log.h> #include <systemlib/mavlink_log.h>
/**
* navigator app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
using namespace time_literals; using namespace time_literals;
namespace navigator namespace navigator
{ {
Navigator *g_navigator; Navigator *g_navigator;
} }
Navigator::Navigator() : Navigator::Navigator() :
@@ -116,8 +111,7 @@ Navigator::~Navigator()
orb_unsubscribe(_vehicle_status_sub); orb_unsubscribe(_vehicle_status_sub);
} }
void void Navigator::params_update()
Navigator::params_update()
{ {
updateParams(); updateParams();
@@ -138,8 +132,7 @@ Navigator::params_update()
} }
} }
void void Navigator::run()
Navigator::run()
{ {
bool have_geofence_position_data = false; bool have_geofence_position_data = false;
@@ -776,7 +769,6 @@ Navigator::run()
void Navigator::geofence_breach_check(bool &have_geofence_position_data) void Navigator::geofence_breach_check(bool &have_geofence_position_data)
{ {
if (have_geofence_position_data && if (have_geofence_position_data &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL_US)) { (hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL_US)) {
@@ -934,8 +926,7 @@ Navigator *Navigator::instantiate(int argc, char *argv[])
return instance; return instance;
} }
int int Navigator::print_status()
Navigator::print_status()
{ {
PX4_INFO("Running"); PX4_INFO("Running");
@@ -943,22 +934,19 @@ Navigator::print_status()
return 0; return 0;
} }
void void Navigator::publish_position_setpoint_triplet()
Navigator::publish_position_setpoint_triplet()
{ {
_pos_sp_triplet.timestamp = hrt_absolute_time(); _pos_sp_triplet.timestamp = hrt_absolute_time();
_pos_sp_triplet_pub.publish(_pos_sp_triplet); _pos_sp_triplet_pub.publish(_pos_sp_triplet);
_pos_sp_triplet_updated = false; _pos_sp_triplet_updated = false;
} }
float float Navigator::get_default_acceptance_radius()
Navigator::get_default_acceptance_radius()
{ {
return _param_nav_acc_rad.get(); return _param_nav_acc_rad.get();
} }
float float Navigator::get_default_altitude_acceptance_radius()
Navigator::get_default_altitude_acceptance_radius()
{ {
if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return _param_nav_fw_alt_rad.get(); return _param_nav_fw_alt_rad.get();
@@ -980,8 +968,7 @@ Navigator::get_default_altitude_acceptance_radius()
} }
} }
float float Navigator::get_altitude_acceptance_radius()
Navigator::get_altitude_acceptance_radius()
{ {
if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next; const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next;
@@ -995,8 +982,7 @@ Navigator::get_altitude_acceptance_radius()
return get_default_altitude_acceptance_radius(); return get_default_altitude_acceptance_radius();
} }
float float Navigator::get_cruising_speed()
Navigator::get_cruising_speed()
{ {
/* there are three options: The mission-requested cruise speed, or the current hover / plane speed */ /* there are three options: The mission-requested cruise speed, or the current hover / plane speed */
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
@@ -1017,8 +1003,7 @@ Navigator::get_cruising_speed()
} }
} }
void void Navigator::set_cruising_speed(float speed)
Navigator::set_cruising_speed(float speed)
{ {
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_mission_cruising_speed_mc = speed; _mission_cruising_speed_mc = speed;
@@ -1028,15 +1013,13 @@ Navigator::set_cruising_speed(float speed)
} }
} }
void void Navigator::reset_cruising_speed()
Navigator::reset_cruising_speed()
{ {
_mission_cruising_speed_mc = -1.0f; _mission_cruising_speed_mc = -1.0f;
_mission_cruising_speed_fw = -1.0f; _mission_cruising_speed_fw = -1.0f;
} }
void void Navigator::reset_triplets()
Navigator::reset_triplets()
{ {
reset_position_setpoint(_pos_sp_triplet.previous); reset_position_setpoint(_pos_sp_triplet.previous);
reset_position_setpoint(_pos_sp_triplet.current); reset_position_setpoint(_pos_sp_triplet.current);
@@ -1045,8 +1028,7 @@ Navigator::reset_triplets()
_pos_sp_triplet_updated = true; _pos_sp_triplet_updated = true;
} }
void void Navigator::reset_position_setpoint(position_setpoint_s &sp)
Navigator::reset_position_setpoint(position_setpoint_s &sp)
{ {
sp = position_setpoint_s{}; sp = position_setpoint_s{};
sp.timestamp = hrt_absolute_time(); sp.timestamp = hrt_absolute_time();
@@ -1061,8 +1043,7 @@ Navigator::reset_position_setpoint(position_setpoint_s &sp)
sp.disable_weather_vane = false; sp.disable_weather_vane = false;
} }
float float Navigator::get_cruising_throttle()
Navigator::get_cruising_throttle()
{ {
/* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */ /* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */
if (_mission_throttle > FLT_EPSILON) { if (_mission_throttle > FLT_EPSILON) {
@@ -1073,8 +1054,7 @@ Navigator::get_cruising_throttle()
} }
} }
float float Navigator::get_acceptance_radius()
Navigator::get_acceptance_radius()
{ {
float acceptance_radius = get_default_acceptance_radius(); // the value specified in the parameter NAV_ACC_RAD float acceptance_radius = get_default_acceptance_radius(); // the value specified in the parameter NAV_ACC_RAD
const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
@@ -1089,8 +1069,7 @@ Navigator::get_acceptance_radius()
return acceptance_radius; return acceptance_radius;
} }
float float Navigator::get_yaw_acceptance(float mission_item_yaw)
Navigator::get_yaw_acceptance(float mission_item_yaw)
{ {
float yaw = mission_item_yaw; float yaw = mission_item_yaw;
@@ -1105,20 +1084,17 @@ Navigator::get_yaw_acceptance(float mission_item_yaw)
return yaw; return yaw;
} }
void void Navigator::load_fence_from_file(const char *filename)
Navigator::load_fence_from_file(const char *filename)
{ {
_geofence.loadFromFile(filename); _geofence.loadFromFile(filename);
} }
/**
* Creates a fake traffic measurement with supplied parameters.
*
*/
void Navigator::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, void Navigator::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,
float altitude_diff, float hor_velocity, float ver_velocity, int emitter_type) float altitude_diff, float hor_velocity, float ver_velocity, int emitter_type)
{ {
double lat, lon; double lat{0.0};
double lon{0.0};
waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, direction, distance, &lat, waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, direction, distance, &lat,
&lon); &lon);
float alt = get_global_position()->alt + altitude_diff; float alt = get_global_position()->alt + altitude_diff;
@@ -1148,9 +1124,6 @@ void Navigator::fake_traffic(const char *callsign, float distance, float directi
transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN); // Flags to indicate various statuses including valid data fields transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN); // Flags to indicate various statuses including valid data fields
tr.squawk = 6667; tr.squawk = 6667;
#ifndef BOARD_HAS_NO_UUID #ifndef BOARD_HAS_NO_UUID
px4_guid_t px4_guid; px4_guid_t px4_guid;
board_get_px4_guid(px4_guid); board_get_px4_guid(px4_guid);
@@ -1370,8 +1343,7 @@ void Navigator::check_traffic()
} }
} }
bool bool Navigator::buffer_air_traffic(uint32_t icao_address)
Navigator::buffer_air_traffic(uint32_t icao_address)
{ {
bool action_needed = true; bool action_needed = true;
@@ -1392,8 +1364,7 @@ Navigator::buffer_air_traffic(uint32_t icao_address)
return action_needed; return action_needed;
} }
bool bool Navigator::abort_landing()
Navigator::abort_landing()
{ {
// only abort if currently landing and position controller status updated // only abort if currently landing and position controller status updated
bool should_abort = false; bool should_abort = false;
@@ -1416,8 +1387,7 @@ Navigator::abort_landing()
return should_abort; return should_abort;
} }
bool bool Navigator::force_vtol()
Navigator::force_vtol()
{ {
return _vstatus.is_vtol && return _vstatus.is_vtol &&
(_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || _vstatus.in_transition_to_fw) (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || _vstatus.in_transition_to_fw)
@@ -1448,13 +1418,7 @@ int Navigator::custom_command(int argc, char *argv[])
return print_usage("unknown command"); return print_usage("unknown command");
} }
int navigator_main(int argc, char *argv[]) void Navigator::publish_mission_result()
{
return Navigator::main(argc, argv);
}
void
Navigator::publish_mission_result()
{ {
_mission_result.timestamp = hrt_absolute_time(); _mission_result.timestamp = hrt_absolute_time();
@@ -1469,8 +1433,7 @@ Navigator::publish_mission_result()
_mission_result_updated = false; _mission_result_updated = false;
} }
void void Navigator::set_mission_failure_heading_timeout()
Navigator::set_mission_failure_heading_timeout()
{ {
if (!_mission_result.failure) { if (!_mission_result.failure) {
_mission_result.failure = true; _mission_result.failure = true;
@@ -1481,8 +1444,7 @@ Navigator::set_mission_failure_heading_timeout()
} }
} }
void void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
{ {
vcmd->timestamp = hrt_absolute_time(); vcmd->timestamp = hrt_absolute_time();
vcmd->source_system = _vstatus.system_id; vcmd->source_system = _vstatus.system_id;
@@ -1509,8 +1471,7 @@ Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
_vehicle_cmd_pub.publish(*vcmd); _vehicle_cmd_pub.publish(*vcmd);
} }
void void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result)
Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result)
{ {
vehicle_command_ack_s command_ack = {}; vehicle_command_ack_s command_ack = {};
@@ -1527,8 +1488,7 @@ Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t res
_vehicle_cmd_ack_pub.publish(command_ack); _vehicle_cmd_ack_pub.publish(command_ack);
} }
void void Navigator::acquire_gimbal_control()
Navigator::acquire_gimbal_control()
{ {
vehicle_command_s vcmd = {}; vehicle_command_s vcmd = {};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
@@ -1539,8 +1499,7 @@ Navigator::acquire_gimbal_control()
publish_vehicle_cmd(&vcmd); publish_vehicle_cmd(&vcmd);
} }
void void Navigator::release_gimbal_control()
Navigator::release_gimbal_control()
{ {
vehicle_command_s vcmd = {}; vehicle_command_s vcmd = {};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
@@ -1595,3 +1554,13 @@ controller.
return 0; return 0;
} }
/**
* navigator app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int navigator_main(int argc, char *argv[])
{
return Navigator::main(argc, argv);
}