diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 46dd0d3da0a82c44375aa6e44344e06e224f67a4..3c92ed41fe6ca9af6b9c27e2fe12fbf30d63994b 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -97,15 +97,15 @@ DataLinkLoss::set_dll_item() switch (_dll_state) { case DLL_STATE_FLYTOCOMMSHOLDWP: { - _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; - _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; + _mission_item.lat = (double)(_param_nav_dll_ch_lat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_nav_dll_ch_lon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _param_commsholdalt.get(); + _mission_item.altitude = _param_nav_dll_ch_alt.get(); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); + _mission_item.time_inside = _param_nav_dll_ch_t.get() < 0.0f ? 0.0f : _param_nav_dll_ch_t.get(); _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; @@ -114,14 +114,14 @@ DataLinkLoss::set_dll_item() } case DLL_STATE_FLYTOAIRFIELDHOMEWP: { - _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; - _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; + _mission_item.lat = (double)(_param_nav_ah_lat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_nav_ah_lon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _param_airfieldhomealt.get(); + _mission_item.altitude = _param_nav_ah_alt.get(); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - _mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get(); + _mission_item.time_inside = _param_nav_dll_ah_t.get() < 0.0f ? 0.0f : _param_nav_dll_ah_t.get(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; @@ -163,9 +163,9 @@ DataLinkLoss::advance_dll() /* Check the number of data link losses. If above home fly home directly */ /* if number of data link losses limit is not reached fly to comms hold wp */ - if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + if (_navigator->get_vstatus()->data_link_lost_counter > _param_nav_dll_n.get()) { warnx("%d data link losses, limit is %d, fly to airfield home", - _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + _navigator->get_vstatus()->data_link_lost_counter, _param_nav_dll_n.get()); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); @@ -173,7 +173,7 @@ DataLinkLoss::advance_dll() _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { - if (!_param_skipcommshold.get()) { + if (!_param_nav_dll_chsk.get()) { warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 292be27f867a3bb30ac2743ca620c460e9e40005..dc4d06dabca5a3bed6dc954ee824d10445d2291e 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -59,16 +59,16 @@ public: private: DEFINE_PARAMETERS( - (ParamFloat<px4::params::NAV_DLL_CH_T>) _param_commsholdwaittime, - (ParamInt<px4::params::NAV_DLL_CH_LAT>) _param_commsholdlat, // * 1e7 - (ParamInt<px4::params::NAV_DLL_CH_LON>) _param_commsholdlon, // * 1e7 - (ParamFloat<px4::params::NAV_DLL_CH_ALT>) _param_commsholdalt, - (ParamInt<px4::params::NAV_AH_LAT>) _param_airfieldhomelat, // * 1e7 - (ParamInt<px4::params::NAV_AH_LON>) _param_airfieldhomelon, // * 1e7 - (ParamFloat<px4::params::NAV_AH_ALT>) _param_airfieldhomealt, - (ParamFloat<px4::params::NAV_DLL_AH_T>) _param_airfieldhomewaittime, - (ParamInt<px4::params::NAV_DLL_N>) _param_numberdatalinklosses, - (ParamInt<px4::params::NAV_DLL_CHSK>) _param_skipcommshold + (ParamFloat<px4::params::NAV_DLL_CH_T>) _param_nav_dll_ch_t, + (ParamInt<px4::params::NAV_DLL_CH_LAT>) _param_nav_dll_ch_lat, // * 1e7 + (ParamInt<px4::params::NAV_DLL_CH_LON>) _param_nav_dll_ch_lon, // * 1e7 + (ParamFloat<px4::params::NAV_DLL_CH_ALT>) _param_nav_dll_ch_alt, + (ParamInt<px4::params::NAV_AH_LAT>) _param_nav_ah_lat, // * 1e7 + (ParamInt<px4::params::NAV_AH_LON>) _param_nav_ah_lon, // * 1e7 + (ParamFloat<px4::params::NAV_AH_ALT>) _param_nav_ah_alt, + (ParamFloat<px4::params::NAV_DLL_AH_T>) _param_nav_dll_ah_t, + (ParamInt<px4::params::NAV_DLL_N>) _param_nav_dll_n, + (ParamInt<px4::params::NAV_DLL_CHSK>) _param_nav_dll_chsk ) enum DLLState { diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 53e68a72fa16958d91bbf2291f6806bd389d419f..de1df7f4f59139998d937d6f40f41f80de74dc68 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -79,11 +79,11 @@ void FollowTarget::on_inactive() void FollowTarget::on_activation() { - _follow_offset = _param_tracking_dist.get() < 1.0F ? 1.0F : _param_tracking_dist.get(); + _follow_offset = _param_nav_ft_dst.get() < 1.0F ? 1.0F : _param_nav_ft_dst.get(); - _responsiveness = math::constrain((float) _param_tracking_resp.get(), .1F, 1.0F); + _responsiveness = math::constrain((float) _param_nav_ft_rs.get(), .1F, 1.0F); - _follow_target_position = _param_tracking_side.get(); + _follow_target_position = _param_nav_ft_fs.get(); if ((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) { _follow_target_position = FOLLOW_FROM_BEHIND; @@ -250,7 +250,7 @@ void FollowTarget::on_active() _follow_target_state = TRACK_VELOCITY; } else if (target_velocity_valid()) { - set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); + set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle); // keep the current velocity updated with the target velocity for when it's needed _current_vel = _est_target_vel; @@ -275,7 +275,7 @@ void FollowTarget::on_active() _last_update_time = current_time; } - set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); + set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle); update_position_sp(true, false, _yaw_rate); @@ -299,7 +299,7 @@ void FollowTarget::on_active() target.lon = _navigator->get_global_position()->lon; target.alt = 0.0F; - set_follow_target_item(&_mission_item, _param_min_alt.get(), target, _yaw_angle); + set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target, _yaw_angle); update_position_sp(false, false, _yaw_rate); diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index e3d000055192846f1e556ca711970b6201efb656..9119d84e5c9c727feab0f06ee304fc4278dcec37 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -90,10 +90,10 @@ private: }; DEFINE_PARAMETERS( - (ParamFloat<px4::params::NAV_MIN_FT_HT>) _param_min_alt, - (ParamFloat<px4::params::NAV_FT_DST>) _param_tracking_dist, - (ParamInt<px4::params::NAV_FT_FS>) _param_tracking_side, - (ParamFloat<px4::params::NAV_FT_RS>) _param_tracking_resp + (ParamFloat<px4::params::NAV_MIN_FT_HT>) _param_nav_min_ft_ht, + (ParamFloat<px4::params::NAV_FT_DST>) _param_nav_ft_dst, + (ParamInt<px4::params::NAV_FT_FS>) _param_nav_ft_fs, + (ParamFloat<px4::params::NAV_FT_RS>) _param_nav_ft_rs ) FollowTargetState _follow_target_state{SET_WAIT_FOR_TARGET_POSITION}; diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index a183ac4a55f85fd5b79fe6ded696d75aeeefef78..e07965cedffb0ec14fe56ff3897b629a7fa220cf 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -224,8 +224,8 @@ bool Geofence::checkAll(double lat, double lon, float altitude) if (isHomeRequired() && _navigator->home_position_valid()) { - const float max_horizontal_distance = _param_max_hor_distance.get(); - const float max_vertical_distance = _param_max_ver_distance.get(); + const float max_horizontal_distance = _param_gf_max_hor_dist.get(); + const float max_vertical_distance = _param_gf_max_ver_dist.get(); const double home_lat = _navigator->get_home_position()->lat; const double home_lon = _navigator->get_home_position()->lon; @@ -268,7 +268,7 @@ bool Geofence::checkAll(double lat, double lon, float altitude) } else { _outside_counter++; - if (_outside_counter > _param_counter_threshold.get()) { + if (_outside_counter > _param_gf_count.get()) { return inside_fence; } else { @@ -568,8 +568,8 @@ int Geofence::clearDm() bool Geofence::isHomeRequired() { - bool max_horizontal_enabled = (_param_max_hor_distance.get() > FLT_EPSILON); - bool max_vertical_enabled = (_param_max_ver_distance.get() > FLT_EPSILON); + bool max_horizontal_enabled = (_param_gf_max_hor_dist.get() > FLT_EPSILON); + bool max_vertical_enabled = (_param_gf_max_ver_dist.get() > FLT_EPSILON); bool geofence_action_rtl = (getGeofenceAction() == geofence_result_s::GF_ACTION_RTL); return max_horizontal_enabled || max_vertical_enabled || geofence_action_rtl; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 9a5412bbaaec0e6dbe4cb6b5dc74ff16276e2f4d..f9ed6f8ac43c989031df3aa0e148e647390a56cc 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -124,9 +124,9 @@ public: bool isEmpty() { return _num_polygons == 0; } - int getAltitudeMode() { return _param_altitude_mode.get(); } - int getSource() { return _param_source.get(); } - int getGeofenceAction() { return _param_action.get(); } + int getAltitudeMode() { return _param_gf_altmode.get(); } + int getSource() { return _param_gf_source.get(); } + int getGeofenceAction() { return _param_gf_action.get(); } bool isHomeRequired(); @@ -158,12 +158,12 @@ private: map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m] DEFINE_PARAMETERS( - (ParamInt<px4::params::GF_ACTION>) _param_action, - (ParamInt<px4::params::GF_ALTMODE>) _param_altitude_mode, - (ParamInt<px4::params::GF_SOURCE>) _param_source, - (ParamInt<px4::params::GF_COUNT>) _param_counter_threshold, - (ParamFloat<px4::params::GF_MAX_HOR_DIST>) _param_max_hor_distance, - (ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_max_ver_distance + (ParamInt<px4::params::GF_ACTION>) _param_gf_action, + (ParamInt<px4::params::GF_ALTMODE>) _param_gf_altmode, + (ParamInt<px4::params::GF_SOURCE>) _param_gf_source, + (ParamInt<px4::params::GF_COUNT>) _param_gf_count, + (ParamFloat<px4::params::GF_MAX_HOR_DIST>) _param_gf_max_hor_dist, + (ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_gf_max_ver_dist ) uORB::Subscription<vehicle_air_data_s> _sub_airdata; diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 94bdf97a38363091d8cc0868a91995319e6586bd..c659e3f4b440022af5f8b9463ec8d749adcb8c60 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -85,9 +85,9 @@ GpsFailure::on_active() * navigator has to publish an attitude setpoint */ vehicle_attitude_setpoint_s att_sp = {}; att_sp.timestamp = hrt_absolute_time(); - att_sp.roll_body = math::radians(_param_openlooploiter_roll.get()); - att_sp.pitch_body = math::radians(_param_openlooploiter_pitch.get()); - att_sp.thrust_body[0] = _param_openlooploiter_thrust.get(); + att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); + att_sp.pitch_body = math::radians(_param_nav_gpsf_p.get()); + att_sp.thrust_body[0] = _param_nav_gpsf_tr.get(); Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f)); q.copyTo(att_sp.q_d); @@ -103,8 +103,8 @@ GpsFailure::on_active() } /* Measure time */ - if ((_param_loitertime.get() > FLT_EPSILON) && - (hrt_elapsed_time(&_timestamp_activation) > _param_loitertime.get() * 1e6f)) { + if ((_param_nav_gpsf_lt.get() > FLT_EPSILON) && + (hrt_elapsed_time(&_timestamp_activation) > _param_nav_gpsf_lt.get() * 1e6f)) { /* no recovery, advance the state machine */ PX4_WARN("GPS not recovered, switching to next failure state"); advance_gpsf(); diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h index 1802031cbc7b0648f7a74e0e7b842f8e7acdaece..a683a7a3d34dc91454304cd9a39ca2cc24ba271e 100644 --- a/src/modules/navigator/gpsfailure.h +++ b/src/modules/navigator/gpsfailure.h @@ -57,10 +57,10 @@ public: private: DEFINE_PARAMETERS( - (ParamFloat<px4::params::NAV_GPSF_LT>) _param_loitertime, - (ParamFloat<px4::params::NAV_GPSF_R>) _param_openlooploiter_roll, - (ParamFloat<px4::params::NAV_GPSF_P>) _param_openlooploiter_pitch, - (ParamFloat<px4::params::NAV_GPSF_TR>) _param_openlooploiter_thrust + (ParamFloat<px4::params::NAV_GPSF_LT>) _param_nav_gpsf_lt, + (ParamFloat<px4::params::NAV_GPSF_R>) _param_nav_gpsf_r, + (ParamFloat<px4::params::NAV_GPSF_P>) _param_nav_gpsf_p, + (ParamFloat<px4::params::NAV_GPSF_TR>) _param_nav_gpsf_tr ) enum GPSFState { diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 8ef26e7831d1607f006a062f7b671021aa473a92..20bb8e8e23c4646d692ed0b20c4ff4f686543397 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -226,7 +226,7 @@ Mission::on_active() set_mission_items(); } - } else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) { + } else if (_mission_type != MISSION_TYPE_NONE && _param_mis_altmode.get() == MISSION_ALTMODE_FOH) { altitude_sp_foh_update(); @@ -243,7 +243,7 @@ Mission::on_active() } /* see if we need to update the current yaw heading */ - if (!_param_mnt_yaw_ctl.get() && (_navigator->get_vstatus()->is_rotary_wing) + if (!_param_mis_mnt_yaw_ctl.get() && (_navigator->get_vstatus()->is_rotary_wing) && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE) && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF @@ -1612,8 +1612,8 @@ Mission::check_mission_valid(bool force) _navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_mission, - _param_dist_1wp.get(), - _param_dist_between_wps.get(), + _param_mis_dist_1wp.get(), + _param_mis_dist_wps.get(), _navigator->mission_landing_required()); _navigator->get_mission_result()->seq_total = _mission.count; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index b90c0b50a08e827db6b76981c9c533b38f640929..2414ccaa64907f880cac3ffa74717ce287488d78 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -236,10 +236,10 @@ private: bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; DEFINE_PARAMETERS( - (ParamFloat<px4::params::MIS_DIST_1WP>) _param_dist_1wp, - (ParamFloat<px4::params::MIS_DIST_WPS>) _param_dist_between_wps, - (ParamInt<px4::params::MIS_ALTMODE>) _param_altmode, - (ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mnt_yaw_ctl + (ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp, + (ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps, + (ParamInt<px4::params::MIS_ALTMODE>) _param_mis_altmode, + (ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mis_mnt_yaw_ctl ) struct mission_s _mission {}; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index c83bb58870576a888812c35ec732c29725770ca0..1e7afae7c6f8960bb44c9ba0963c23e4166c0fee 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -169,7 +169,7 @@ public: Geofence &get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } - float get_loiter_radius() { return _param_loiter_radius.get(); } + float get_loiter_radius() { return _param_nav_loiter_rad.get(); } /** * Returns the default acceptance radius defined by the parameter @@ -279,11 +279,11 @@ public: bool abort_landing(); // Param access - float get_loiter_min_alt() const { return _param_loiter_min_alt.get(); } - float get_takeoff_min_alt() const { return _param_takeoff_min_alt.get(); } - bool get_takeoff_required() const { return _param_takeoff_required.get(); } - float get_yaw_timeout() const { return _param_yaw_timeout.get(); } - float get_yaw_threshold() const { return _param_yaw_err.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(); } + 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_threshold() const { return _param_mis_yaw_err.get(); } float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; } float get_vtol_reverse_delay() const { return _param_reverse_delay; } @@ -357,24 +357,24 @@ private: NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ DEFINE_PARAMETERS( - (ParamFloat<px4::params::NAV_LOITER_RAD>) _param_loiter_radius, /**< loiter radius for fixedwing */ - (ParamFloat<px4::params::NAV_ACC_RAD>) _param_acceptance_radius, /**< acceptance for takeoff */ + (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_fw_alt_acceptance_radius, /**< acceptance radius for fixedwing altitude */ + _param_nav_fw_alt_rad, /**< acceptance radius for fixedwing altitude */ (ParamFloat<px4::params::NAV_FW_ALTL_RAD>) - _param_fw_alt_lnd_acceptance_radius, /**< acceptance radius for fixedwing altitude before landing*/ + _param_nav_fw_altl_rad, /**< acceptance radius for fixedwing altitude before landing*/ (ParamFloat<px4::params::NAV_MC_ALT_RAD>) - _param_mc_alt_acceptance_radius, /**< acceptance radius for multicopter altitude */ - (ParamInt<px4::params::NAV_FORCE_VT>) _param_force_vtol, /**< acceptance radius for multicopter altitude */ - (ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_traffic_avoidance_mode, /**< avoiding other aircraft is enabled */ + _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 */ // non-navigator parameters // Mission (MIS_*) - (ParamFloat<px4::params::MIS_LTRMIN_ALT>) _param_loiter_min_alt, - (ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_takeoff_min_alt, - (ParamBool<px4::params::MIS_TAKEOFF_REQ>) _param_takeoff_required, - (ParamFloat<px4::params::MIS_YAW_TMT>) _param_yaw_timeout, - (ParamFloat<px4::params::MIS_YAW_ERR>) _param_yaw_err + (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 ) param_t _handle_back_trans_dec_mss{PARAM_INVALID}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7cd439e2b0a84698fe4fc3da7a6b6062cfc90673..88459620997711a6ad9bdec1e981f90f0458af50 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -856,23 +856,23 @@ Navigator::publish_position_setpoint_triplet() float Navigator::get_default_acceptance_radius() { - return _param_acceptance_radius.get(); + return _param_nav_acc_rad.get(); } float Navigator::get_acceptance_radius() { - return get_acceptance_radius(_param_acceptance_radius.get()); + return get_acceptance_radius(_param_nav_acc_rad.get()); } float Navigator::get_default_altitude_acceptance_radius() { if (!get_vstatus()->is_rotary_wing) { - return _param_fw_alt_acceptance_radius.get(); + return _param_nav_fw_alt_rad.get(); } else { - float alt_acceptance_radius = _param_mc_alt_acceptance_radius.get(); + float alt_acceptance_radius = _param_nav_mc_alt_rad.get(); const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); @@ -893,7 +893,7 @@ Navigator::get_altitude_acceptance_radius() if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) { // Use separate (tighter) altitude acceptance for clean altitude starting point before landing - return _param_fw_alt_lnd_acceptance_radius.get(); + return _param_nav_fw_altl_rad.get(); } } @@ -1109,7 +1109,7 @@ void Navigator::check_traffic() // direction of traffic in human-readable 0..360 degree in earth frame int traffic_direction = math::degrees(tr.heading) + 180; - switch (_param_traffic_avoidance_mode.get()) { + switch (_param_nav_traff_avoid.get()) { case 0: { /* ignore */ @@ -1182,7 +1182,7 @@ Navigator::force_vtol() { return _vstatus.is_vtol && (!_vstatus.is_rotary_wing || _vstatus.in_transition_to_fw) - && _param_force_vtol.get(); + && _param_nav_force_vt.get(); } int Navigator::print_usage(const char *reason) diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 13ae2ae1905c00d7b81c4761e79510041d28faf8..5294bfba9934d29e821b4ffa5f04f41a90358ff8 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -118,7 +118,7 @@ PrecLand::on_active() _target_pose_valid = true; } - if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) > _param_timeout.get()) { + if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) > _param_pld_btout.get()) { _target_pose_valid = false; } @@ -203,7 +203,7 @@ PrecLand::run_state_start() } // if we don't see the target after 1 second, search for it - if (_param_search_timeout.get() > 0) { + if (_param_pld_srch_tout.get() > 0) { if (hrt_absolute_time() - _point_reached_time > 2000000) { if (!switch_to_state_search()) { @@ -353,7 +353,7 @@ PrecLand::run_state_search() } // check if search timed out and go to fallback - if (hrt_absolute_time() - _state_start_time > _param_search_timeout.get()*SEC2USEC) { + if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get()*SEC2USEC) { PX4_WARN("Search timed out"); if (!switch_to_state_fallback()) { @@ -434,7 +434,7 @@ PrecLand::switch_to_state_search() vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->current.alt = vehicle_local_position->ref_alt + _param_search_alt.get(); + pos_sp_triplet->current.alt = vehicle_local_position->ref_alt + _param_pld_srch_alt.get(); pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; _navigator->set_position_setpoint_triplet_updated(); @@ -475,14 +475,14 @@ bool PrecLand::check_state_conditions(PrecLandState state) switch (state) { case PrecLandState::Start: - return _search_cnt <= _param_max_searches.get(); + return _search_cnt <= _param_pld_max_srch.get(); case PrecLandState::HorizontalApproach: // if we're already in this state, only want to make it invalid if we reached the target but can't see it anymore if (_state == PrecLandState::HorizontalApproach) { - if (fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_hacc_rad.get() - && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_hacc_rad.get()) { + if (fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get() + && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get()) { // we've reached the position where we last saw the target. If we don't see it now, we need to do something return _target_pose_valid && _target_pose.abs_pos_valid; @@ -511,13 +511,13 @@ bool PrecLand::check_state_conditions(PrecLandState state) } else { // if not already in this state, need to be above target to enter it return _target_pose_updated && _target_pose.abs_pos_valid - && fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_hacc_rad.get() - && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_hacc_rad.get(); + && fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get() + && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get(); } case PrecLandState::FinalApproach: return _target_pose_valid && _target_pose.abs_pos_valid - && (_target_pose.z_abs - vehicle_local_position->z) < _param_final_approach_alt.get(); + && (_target_pose.z_abs - vehicle_local_position->z) < _param_pld_fappr_alt.get(); case PrecLandState::Search: return true; diff --git a/src/modules/navigator/precland.h b/src/modules/navigator/precland.h index 1bc205aa8f2c499afcbcda36381480888bebf4a8..452244d408242fe0b0ecd8685b9ecf02d036a417 100644 --- a/src/modules/navigator/precland.h +++ b/src/modules/navigator/precland.h @@ -125,12 +125,12 @@ private: PrecLandMode _mode{PrecLandMode::Opportunistic}; DEFINE_PARAMETERS( - (ParamFloat<px4::params::PLD_BTOUT>) _param_timeout, - (ParamFloat<px4::params::PLD_HACC_RAD>) _param_hacc_rad, - (ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_final_approach_alt, - (ParamFloat<px4::params::PLD_SRCH_ALT>) _param_search_alt, - (ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_search_timeout, - (ParamInt<px4::params::PLD_MAX_SRCH>) _param_max_searches + (ParamFloat<px4::params::PLD_BTOUT>) _param_pld_btout, + (ParamFloat<px4::params::PLD_HACC_RAD>) _param_pld_hacc_rad, + (ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_pld_fappr_alt, + (ParamFloat<px4::params::PLD_SRCH_ALT>) _param_pld_srch_alt, + (ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_pld_srch_tout, + (ParamInt<px4::params::PLD_MAX_SRCH>) _param_pld_max_srch ) // non-navigator parameters diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 68a3633bdb890d2ad561ee8190b466b5b3e55315..d12168c7209628a3055775cf76824f368c0d0d9b 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -104,7 +104,7 @@ RCLoss::set_rcl_item() _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get(); + _mission_item.time_inside = _param_nav_gpsf_lt.get() < 0.0f ? 0.0f : _param_nav_gpsf_lt.get(); _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; @@ -142,7 +142,7 @@ RCLoss::advance_rcl() { switch (_rcl_state) { case RCL_STATE_NONE: - if (_param_loitertime.get() > 0.0f) { + if (_param_nav_gpsf_lt.get() > 0.0f) { warnx("RC loss, OBC mode, loiter"); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RC loss, loitering"); _rcl_state = RCL_STATE_LOITER; diff --git a/src/modules/navigator/rcloss.h b/src/modules/navigator/rcloss.h index c626f1e3124f625145e1cdd0341a335dd302b9f1..561d457f3a7250ca425c9d2e278051e605344df8 100644 --- a/src/modules/navigator/rcloss.h +++ b/src/modules/navigator/rcloss.h @@ -58,7 +58,7 @@ public: private: DEFINE_PARAMETERS( - (ParamFloat<px4::params::NAV_RCL_LT>) _param_loitertime + (ParamFloat<px4::params::NAV_RCL_LT>) _param_nav_gpsf_lt ) enum RCLState { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 16a08092951cc53f9a028fd20a8bfe2227a13d7d..5ad35a21a30f95a236134dc27ddf4a7bcc046457 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -74,7 +74,7 @@ RTL::on_activation() } else if ((rtl_type() == RTL_LAND) && _navigator->on_mission_landing()) { // RTL straight to RETURN state, but mission will takeover for landing. - } else if ((_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) + } else if ((_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_rtl_return_alt.get()) || _rtl_alt_min) { // If lower than return altitude, climb up first. @@ -134,15 +134,15 @@ RTL::set_rtl_item() const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon); // Compute the return altitude. - float return_alt = math::max(home.alt + _param_return_alt.get(), gpos.alt); + float return_alt = math::max(home.alt + _param_rtl_return_alt.get(), gpos.alt); // We are close to home, limit climb to min. if (home_dist < _param_rtl_min_dist.get()) { - return_alt = home.alt + _param_descend_alt.get(); + return_alt = home.alt + _param_rtl_descend_alt.get(); } // Compute the loiter altitude. - const float loiter_altitude = math::min(home.alt + _param_descend_alt.get(), gpos.alt); + const float loiter_altitude = math::min(home.alt + _param_rtl_descend_alt.get(), gpos.alt); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -229,7 +229,7 @@ RTL::set_rtl_item() } case RTL_STATE_LOITER: { - const bool autoland = (_param_land_delay.get() > FLT_EPSILON); + const bool autoland = (_param_rtl_land_delay.get() > FLT_EPSILON); // Don't change altitude. _mission_item.lat = home.lat; @@ -239,7 +239,7 @@ RTL::set_rtl_item() _mission_item.yaw = home.yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = math::max(_param_land_delay.get(), 0.0f); + _mission_item.time_inside = math::max(_param_rtl_land_delay.get(), 0.0f); _mission_item.autocontinue = autoland; _mission_item.origin = ORIGIN_ONBOARD; @@ -324,7 +324,7 @@ RTL::advance_rtl() case RTL_STATE_DESCEND: // Only go to land if autoland is enabled. - if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { + if (_param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA) { _rtl_state = RTL_STATE_LOITER; } else { diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index f477713b86c30bb8b70c0069b41682a70655bc05..d852232d5105ace160837a532f93872a7aaa6d62 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -94,9 +94,9 @@ private: bool _rtl_alt_min{false}; DEFINE_PARAMETERS( - (ParamFloat<px4::params::RTL_RETURN_ALT>) _param_return_alt, - (ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_descend_alt, - (ParamFloat<px4::params::RTL_LAND_DELAY>) _param_land_delay, + (ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt, + (ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_rtl_descend_alt, + (ParamFloat<px4::params::RTL_LAND_DELAY>) _param_rtl_land_delay, (ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist, (ParamInt<px4::params::RTL_TYPE>) _param_rtl_type )