navigator: use new Param class

This commit is contained in:
Beat Küng
2018-03-13 17:35:15 +01:00
parent 32eaf278ad
commit f2dddc65a5
30 changed files with 172 additions and 207 deletions
+3 -12
View File
@@ -54,18 +54,9 @@
#include "navigator.h" #include "navigator.h"
#include "datalinkloss.h" #include "datalinkloss.h"
DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : DataLinkLoss::DataLinkLoss(Navigator *navigator) :
MissionBlock(navigator, name), MissionBlock(navigator),
_param_commsholdwaittime(this, "CH_T"), ModuleParams(navigator),
_param_commsholdlat(this, "CH_LAT"),
_param_commsholdlon(this, "CH_LON"),
_param_commsholdalt(this, "CH_ALT"),
_param_airfieldhomelat(this, "NAV_AH_LAT", false),
_param_airfieldhomelon(this, "NAV_AH_LON", false),
_param_airfieldhomealt(this, "NAV_AH_ALT", false),
_param_airfieldhomewaittime(this, "AH_T"),
_param_numberdatalinklosses(this, "N"),
_param_skipcommshold(this, "CHSK"),
_dll_state(DLL_STATE_NONE) _dll_state(DLL_STATE_NONE)
{ {
} }
+16 -13
View File
@@ -40,15 +40,17 @@
#ifndef NAVIGATOR_DATALINKLOSS_H #ifndef NAVIGATOR_DATALINKLOSS_H
#define NAVIGATOR_DATALINKLOSS_H #define NAVIGATOR_DATALINKLOSS_H
#include <px4_module_params.h>
#include "navigator_mode.h" #include "navigator_mode.h"
#include "mission_block.h" #include "mission_block.h"
class Navigator; class Navigator;
class DataLinkLoss final : public MissionBlock class DataLinkLoss : public MissionBlock, public ModuleParams
{ {
public: public:
DataLinkLoss(Navigator *navigator, const char *name); DataLinkLoss(Navigator *navigator);
~DataLinkLoss() = default; ~DataLinkLoss() = default;
@@ -57,17 +59,18 @@ public:
void on_active() override; void on_active() override;
private: private:
/* Params */ DEFINE_PARAMETERS(
control::BlockParamFloat _param_commsholdwaittime; (ParamFloat<px4::params::NAV_DLL_CH_T>) _param_commsholdwaittime,
control::BlockParamInt _param_commsholdlat; // * 1e7 (ParamInt<px4::params::NAV_DLL_CH_LAT>) _param_commsholdlat, // * 1e7
control::BlockParamInt _param_commsholdlon; // * 1e7 (ParamInt<px4::params::NAV_DLL_CH_LON>) _param_commsholdlon, // * 1e7
control::BlockParamFloat _param_commsholdalt; (ParamFloat<px4::params::NAV_DLL_CH_ALT>) _param_commsholdalt,
control::BlockParamInt _param_airfieldhomelat; // * 1e7 (ParamInt<px4::params::NAV_AH_LAT>) _param_airfieldhomelat, // * 1e7
control::BlockParamInt _param_airfieldhomelon; // * 1e7 (ParamInt<px4::params::NAV_AH_LON>) _param_airfieldhomelon, // * 1e7
control::BlockParamFloat _param_airfieldhomealt; (ParamFloat<px4::params::NAV_AH_ALT>) _param_airfieldhomealt,
control::BlockParamFloat _param_airfieldhomewaittime; (ParamFloat<px4::params::NAV_DLL_AH_T>) _param_airfieldhomewaittime,
control::BlockParamInt _param_numberdatalinklosses; (ParamInt<px4::params::NAV_DLL_N>) _param_numberdatalinklosses,
control::BlockParamInt _param_skipcommshold; (ParamInt<px4::params::NAV_DLL_CHSK>) _param_skipcommshold
)
enum DLLState { enum DLLState {
DLL_STATE_NONE = 0, DLL_STATE_NONE = 0,
+2 -2
View File
@@ -52,8 +52,8 @@
#include "navigator.h" #include "navigator.h"
#include "enginefailure.h" #include "enginefailure.h"
EngineFailure::EngineFailure(Navigator *navigator, const char *name) : EngineFailure::EngineFailure(Navigator *navigator) :
MissionBlock(navigator, name), MissionBlock(navigator),
_ef_state(EF_STATE_NONE) _ef_state(EF_STATE_NONE)
{ {
} }
+2 -2
View File
@@ -45,10 +45,10 @@
class Navigator; class Navigator;
class EngineFailure final : public MissionBlock class EngineFailure : public MissionBlock
{ {
public: public:
EngineFailure(Navigator *navigator, const char *name); EngineFailure(Navigator *navigator);
~EngineFailure() = default; ~EngineFailure() = default;
void on_inactive() override; void on_inactive() override;
+3 -6
View File
@@ -58,12 +58,9 @@
constexpr float FollowTarget::_follow_position_matricies[4][9]; constexpr float FollowTarget::_follow_position_matricies[4][9];
FollowTarget::FollowTarget(Navigator *navigator, const char *name) : FollowTarget::FollowTarget(Navigator *navigator) :
MissionBlock(navigator, name), MissionBlock(navigator),
_param_min_alt(this, "NAV_MIN_FT_HT", false), ModuleParams(navigator)
_param_tracking_dist(this, "NAV_FT_DST", false),
_param_tracking_side(this, "NAV_FT_FS", false),
_param_tracking_resp(this, "NAV_FT_RS", false)
{ {
_current_vel.zero(); _current_vel.zero();
_step_vel.zero(); _step_vel.zero();
+9 -6
View File
@@ -45,13 +45,14 @@
#include "navigator_mode.h" #include "navigator_mode.h"
#include "mission_block.h" #include "mission_block.h"
#include <px4_module_params.h>
#include <uORB/topics/follow_target.h> #include <uORB/topics/follow_target.h>
class FollowTarget final : public MissionBlock class FollowTarget : public MissionBlock, public ModuleParams
{ {
public: public:
FollowTarget(Navigator *navigator, const char *name); FollowTarget(Navigator *navigator);
~FollowTarget() = default; ~FollowTarget() = default;
void on_inactive() override; void on_inactive() override;
@@ -107,10 +108,12 @@ private:
}; // follow left side }; // follow left side
control::BlockParamFloat _param_min_alt; DEFINE_PARAMETERS(
control::BlockParamFloat _param_tracking_dist; (ParamFloat<px4::params::NAV_MIN_FT_HT>) _param_min_alt,
control::BlockParamInt _param_tracking_side; (ParamFloat<px4::params::NAV_FT_DST>) _param_tracking_dist,
control::BlockParamFloat _param_tracking_resp; (ParamInt<px4::params::NAV_FT_FS>) _param_tracking_side,
(ParamFloat<px4::params::NAV_FT_RS>) _param_tracking_resp
)
FollowTargetState _follow_target_state{SET_WAIT_FOR_TARGET_POSITION}; FollowTargetState _follow_target_state{SET_WAIT_FOR_TARGET_POSITION};
int _follow_target_position{FOLLOW_FROM_BEHIND}; int _follow_target_position{FOLLOW_FROM_BEHIND};
+2 -8
View File
@@ -53,14 +53,8 @@
#define GEOFENCE_RANGE_WARNING_LIMIT 5000000 #define GEOFENCE_RANGE_WARNING_LIMIT 5000000
Geofence::Geofence(Navigator *navigator) : Geofence::Geofence(Navigator *navigator) :
SuperBlock(navigator, "GF"), ModuleParams(navigator),
_navigator(navigator), _navigator(navigator)
_param_action(this, "GF_ACTION", false),
_param_altitude_mode(this, "GF_ALTMODE", false),
_param_source(this, "GF_SOURCE", false),
_param_counter_threshold(this, "GF_COUNT", false),
_param_max_hor_distance(this, "GF_MAX_HOR_DIST", false),
_param_max_ver_distance(this, "GF_MAX_VER_DIST", false)
{ {
// we assume there's no concurrent fence update on startup // we assume there's no concurrent fence update on startup
_updateFence(); _updateFence();
+10 -11
View File
@@ -43,9 +43,7 @@
#include <cfloat> #include <cfloat>
#include <controllib/blocks.hpp> #include <px4_module_params.h>
#include <controllib/block/BlockParam.hpp>
#include <controllib/blocks.hpp>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <geo/geo.h> #include <geo/geo.h>
#include <px4_defines.h> #include <px4_defines.h>
@@ -57,7 +55,7 @@
class Navigator; class Navigator;
class Geofence : public control::SuperBlock class Geofence : public ModuleParams
{ {
public: public:
Geofence(Navigator *navigator); Geofence(Navigator *navigator);
@@ -159,13 +157,14 @@ private:
map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m] map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
/* Params */ DEFINE_PARAMETERS(
control::BlockParamInt _param_action; (ParamInt<px4::params::GF_ACTION>) _param_action,
control::BlockParamInt _param_altitude_mode; (ParamInt<px4::params::GF_ALTMODE>) _param_altitude_mode,
control::BlockParamInt _param_source; (ParamInt<px4::params::GF_SOURCE>) _param_source,
control::BlockParamInt _param_counter_threshold; (ParamInt<px4::params::GF_COUNT>) _param_counter_threshold,
control::BlockParamFloat _param_max_hor_distance; (ParamFloat<px4::params::GF_MAX_HOR_DIST>) _param_max_hor_distance,
control::BlockParamFloat _param_max_ver_distance; (ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_max_ver_distance
)
int _outside_counter{0}; int _outside_counter{0};
uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated
+3 -6
View File
@@ -51,12 +51,9 @@
using matrix::Eulerf; using matrix::Eulerf;
using matrix::Quatf; using matrix::Quatf;
GpsFailure::GpsFailure(Navigator *navigator, const char *name) : GpsFailure::GpsFailure(Navigator *navigator) :
MissionBlock(navigator, name), MissionBlock(navigator),
_param_loitertime(this, "LT"), ModuleParams(navigator)
_param_openlooploiter_roll(this, "R"),
_param_openlooploiter_pitch(this, "P"),
_param_openlooploiter_thrust(this, "TR")
{ {
} }
+10 -7
View File
@@ -40,14 +40,16 @@
#ifndef NAVIGATOR_GPSFAILURE_H #ifndef NAVIGATOR_GPSFAILURE_H
#define NAVIGATOR_GPSFAILURE_H #define NAVIGATOR_GPSFAILURE_H
#include <px4_module_params.h>
#include "mission_block.h" #include "mission_block.h"
class Navigator; class Navigator;
class GpsFailure final : public MissionBlock class GpsFailure : public MissionBlock, public ModuleParams
{ {
public: public:
GpsFailure(Navigator *navigator, const char *name); GpsFailure(Navigator *navigator);
~GpsFailure() = default; ~GpsFailure() = default;
void on_inactive() override; void on_inactive() override;
@@ -55,11 +57,12 @@ public:
void on_active() override; void on_active() override;
private: private:
/* Params */ DEFINE_PARAMETERS(
control::BlockParamFloat _param_loitertime; (ParamFloat<px4::params::NAV_GPSF_LT>) _param_loitertime,
control::BlockParamFloat _param_openlooploiter_roll; (ParamFloat<px4::params::NAV_GPSF_R>) _param_openlooploiter_roll,
control::BlockParamFloat _param_openlooploiter_pitch; (ParamFloat<px4::params::NAV_GPSF_P>) _param_openlooploiter_pitch,
control::BlockParamFloat _param_openlooploiter_thrust; (ParamFloat<px4::params::NAV_GPSF_TR>) _param_openlooploiter_thrust
)
enum GPSFState { enum GPSFState {
GPSF_STATE_NONE = 0, GPSF_STATE_NONE = 0,
+2 -2
View File
@@ -41,8 +41,8 @@
#include "land.h" #include "land.h"
#include "navigator.h" #include "navigator.h"
Land::Land(Navigator *navigator, const char *name) : Land::Land(Navigator *navigator) :
MissionBlock(navigator, name) MissionBlock(navigator)
{ {
} }
+1 -1
View File
@@ -47,7 +47,7 @@
class Land final : public MissionBlock class Land final : public MissionBlock
{ {
public: public:
Land(Navigator *navigator, const char *name); Land(Navigator *navigator);
~Land() = default; ~Land() = default;
void on_activation() override; void on_activation() override;
+3 -3
View File
@@ -42,9 +42,9 @@
#include "loiter.h" #include "loiter.h"
#include "navigator.h" #include "navigator.h"
Loiter::Loiter(Navigator *navigator, const char *name) : Loiter::Loiter(Navigator *navigator) :
MissionBlock(navigator, name), MissionBlock(navigator),
_param_yawmode(this, "MIS_YAWMODE", false) ModuleParams(navigator)
{ {
} }
+7 -3
View File
@@ -44,10 +44,12 @@
#include "navigator_mode.h" #include "navigator_mode.h"
#include "mission_block.h" #include "mission_block.h"
class Loiter final : public MissionBlock #include <px4_module_params.h>
class Loiter : public MissionBlock, public ModuleParams
{ {
public: public:
Loiter(Navigator *navigator, const char *name); Loiter(Navigator *navigator);
~Loiter() = default; ~Loiter() = default;
void on_inactive() override; void on_inactive() override;
@@ -75,7 +77,9 @@ private:
*/ */
void set_loiter_position(); void set_loiter_position();
control::BlockParamInt _param_yawmode; DEFINE_PARAMETERS(
(ParamInt<px4::params::MIS_YAWMODE>) _param_yawmode
)
bool _loiter_pos_set{false}; bool _loiter_pos_set{false};
}; };
+3 -7
View File
@@ -59,13 +59,9 @@
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h> #include <uORB/topics/mission_result.h>
Mission::Mission(Navigator *navigator, const char *name) : Mission::Mission(Navigator *navigator) :
MissionBlock(navigator, name), MissionBlock(navigator),
_param_dist_1wp(this, "MIS_DIST_1WP", false), ModuleParams(navigator)
_param_dist_between_wps(this, "MIS_DIST_WPS", false),
_param_altmode(this, "MIS_ALTMODE", false),
_param_yawmode(this, "MIS_YAWMODE", false),
_param_mnt_yaw_ctl(this, "MIS_MNT_YAW_CTL", false)
{ {
} }
+10 -7
View File
@@ -53,6 +53,7 @@
#include <dataman/dataman.h> #include <dataman/dataman.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <px4_module_params.h>
#include <uORB/topics/home_position.h> #include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h> #include <uORB/topics/mission_result.h>
@@ -64,10 +65,10 @@
class Navigator; class Navigator;
class Mission final : public MissionBlock class Mission : public MissionBlock, public ModuleParams
{ {
public: public:
Mission(Navigator *navigator, const char *name); Mission(Navigator *navigator);
~Mission() override = default; ~Mission() override = default;
void on_inactive() override; void on_inactive() override;
@@ -227,11 +228,13 @@ private:
*/ */
bool find_offboard_land_start(); bool find_offboard_land_start();
control::BlockParamFloat _param_dist_1wp; DEFINE_PARAMETERS(
control::BlockParamFloat _param_dist_between_wps; (ParamFloat<px4::params::MIS_DIST_1WP>) _param_dist_1wp,
control::BlockParamInt _param_altmode; (ParamFloat<px4::params::MIS_DIST_WPS>) _param_dist_between_wps,
control::BlockParamInt _param_yawmode; (ParamInt<px4::params::MIS_ALTMODE>) _param_altmode,
control::BlockParamInt _param_mnt_yaw_ctl; (ParamInt<px4::params::MIS_YAWMODE>) _param_yawmode,
(ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mnt_yaw_ctl
)
struct mission_s _offboard_mission {}; struct mission_s _offboard_mission {};
+2 -2
View File
@@ -54,8 +54,8 @@
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vtol_vehicle_status.h> #include <uORB/topics/vtol_vehicle_status.h>
MissionBlock::MissionBlock(Navigator *navigator, const char *name) : MissionBlock::MissionBlock(Navigator *navigator) :
NavigatorMode(navigator, name) NavigatorMode(navigator)
{ {
} }
+1 -3
View File
@@ -44,8 +44,6 @@
#include "navigator_mode.h" #include "navigator_mode.h"
#include "navigation.h" #include "navigation.h"
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h> #include <systemlib/mavlink_log.h>
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>
@@ -62,7 +60,7 @@ public:
/** /**
* Constructor * Constructor
*/ */
MissionBlock(Navigator *navigator, const char *name); MissionBlock(Navigator *navigator);
virtual ~MissionBlock() = default; virtual ~MissionBlock() = default;
MissionBlock(const MissionBlock &) = delete; MissionBlock(const MissionBlock &) = delete;
+21 -19
View File
@@ -56,9 +56,8 @@
#include "rtl.h" #include "rtl.h"
#include "takeoff.h" #include "takeoff.h"
#include <controllib/block/BlockParam.hpp>
#include <controllib/blocks.hpp>
#include <navigator/navigation.h> #include <navigator/navigation.h>
#include <px4_module_params.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <uORB/topics/fw_pos_ctrl_status.h> #include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/geofence_result.h> #include <uORB/topics/geofence_result.h>
@@ -80,7 +79,7 @@
#define NAVIGATOR_MODE_ARRAY_SIZE 11 #define NAVIGATOR_MODE_ARRAY_SIZE 11
class Navigator : public control::SuperBlock class Navigator : public ModuleParams
{ {
public: public:
Navigator(); Navigator();
@@ -331,24 +330,27 @@ private:
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
// navigator parameters DEFINE_PARAMETERS(
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ (ParamFloat<px4::params::NAV_LOITER_RAD>) _param_loiter_radius, /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ (ParamFloat<px4::params::NAV_ACC_RAD>) _param_acceptance_radius, /**< acceptance for takeoff */
control::BlockParamFloat _param_fw_alt_acceptance_radius; /**< acceptance radius for fixedwing altitude */ (ParamFloat<px4::params::NAV_FW_ALT_RAD>)
control::BlockParamFloat _param_mc_alt_acceptance_radius; /**< acceptance radius for multicopter altitude */ _param_fw_alt_acceptance_radius, /**< acceptance radius for fixedwing altitude */
control::BlockParamInt _param_force_vtol; /**< acceptance radius for multicopter altitude */ (ParamFloat<px4::params::NAV_MC_ALT_RAD>)
control::BlockParamInt _param_traffic_avoidance_mode; /**< avoiding other aircraft is enabled */ _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 */
// non-navigator parameters // non-navigator parameters
// Mission (MIS_*) // Mission (MIS_*)
control::BlockParamFloat _param_loiter_min_alt; (ParamFloat<px4::params::MIS_LTRMIN_ALT>) _param_loiter_min_alt,
control::BlockParamFloat _param_takeoff_min_alt; (ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_takeoff_min_alt,
control::BlockParamFloat _param_yaw_timeout; (ParamFloat<px4::params::MIS_YAW_TMT>) _param_yaw_timeout,
control::BlockParamFloat _param_yaw_err; (ParamFloat<px4::params::MIS_YAW_ERR>) _param_yaw_err,
// VTOL parameters TODO: get these out of navigator // VTOL parameters TODO: get these out of navigator
control::BlockParamFloat _param_back_trans_dec_mss; (ParamFloat<px4::params::VT_B_DEC_MSS>) _param_back_trans_dec_mss,
control::BlockParamFloat _param_reverse_delay; (ParamFloat<px4::params::VT_B_REV_DEL>) _param_reverse_delay
)
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};
+12 -26
View File
@@ -84,34 +84,20 @@ Navigator *g_navigator;
} }
Navigator::Navigator() : Navigator::Navigator() :
SuperBlock(nullptr, "NAV"), ModuleParams(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
_geofence(this), _geofence(this),
_mission(this, "MIS"), _mission(this),
_loiter(this, "LOI"), _loiter(this),
_takeoff(this, "TKF"), _takeoff(this),
_land(this, "LND"), _land(this),
_precland(this, "PLD"), _precland(this),
_rtl(this, "RTL"), _rtl(this),
_rcLoss(this, "RCL"), _rcLoss(this),
_dataLinkLoss(this, "DLL"), _dataLinkLoss(this),
_engineFailure(this, "EF"), _engineFailure(this),
_gpsFailure(this, "GPSF"), _gpsFailure(this),
_follow_target(this, "TAR"), _follow_target(this)
// navigator params
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD"),
_param_fw_alt_acceptance_radius(this, "FW_ALT_RAD"),
_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"),
_param_force_vtol(this, "FORCE_VT"),
_param_traffic_avoidance_mode(this, "TRAFF_AVOID"),
// non-navigator params
_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
_param_takeoff_min_alt(this, "MIS_TAKEOFF_ALT", false),
_param_yaw_timeout(this, "MIS_YAW_TMT", false),
_param_yaw_err(this, "MIS_YAW_ERR", false),
_param_back_trans_dec_mss(this, "VT_B_DEC_MSS", false),
_param_reverse_delay(this, "VT_B_REV_DEL", false)
{ {
/* Create a list of our possible navigation types */ /* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission; _navigation_mode_array[0] = &_mission;
+1 -2
View File
@@ -42,8 +42,7 @@
#include "navigator_mode.h" #include "navigator_mode.h"
#include "navigator.h" #include "navigator.h"
NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : NavigatorMode::NavigatorMode(Navigator *navigator) :
Block(navigator, name),
_navigator(navigator), _navigator(navigator),
_active(false) _active(false)
{ {
+2 -5
View File
@@ -42,15 +42,12 @@
#ifndef NAVIGATOR_MODE_H #ifndef NAVIGATOR_MODE_H
#define NAVIGATOR_MODE_H #define NAVIGATOR_MODE_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
class Navigator; class Navigator;
class NavigatorMode : public control::Block class NavigatorMode
{ {
public: public:
NavigatorMode(Navigator *navigator, const char *name); NavigatorMode(Navigator *navigator);
virtual ~NavigatorMode() = default; virtual ~NavigatorMode() = default;
NavigatorMode(const NavigatorMode &) = delete; NavigatorMode(const NavigatorMode &) = delete;
NavigatorMode operator=(const NavigatorMode &) = delete; NavigatorMode operator=(const NavigatorMode &) = delete;
+4 -14
View File
@@ -59,26 +59,16 @@
#define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state #define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state
PrecLand::PrecLand(Navigator *navigator, const char *name) : PrecLand::PrecLand(Navigator *navigator) :
MissionBlock(navigator, name), MissionBlock(navigator),
ModuleParams(navigator),
_targetPoseSub(0), _targetPoseSub(0),
_target_pose_valid(false), _target_pose_valid(false),
_state_start_time(0), _state_start_time(0),
_search_cnt(0), _search_cnt(0),
_approach_alt(0), _approach_alt(0)
_param_timeout(this, "PLD_BTOUT", false),
_param_hacc_rad(this, "PLD_HACC_RAD", false),
_param_final_approach_alt(this, "PLD_FAPPR_ALT", false),
_param_search_alt(this, "PLD_SRCH_ALT", false),
_param_search_timeout(this, "PLD_SRCH_TOUT", false),
_param_max_searches(this, "PLD_MAX_SRCH", false),
_param_acceleration_hor(this, "MPC_ACC_HOR", false),
_param_xy_vel_cruise(this, "MPC_XY_CRUISE", false)
{ {
/* load initial params */
updateParams();
} }
PrecLand::~PrecLand() PrecLand::~PrecLand()
+15 -13
View File
@@ -41,10 +41,10 @@
#ifndef NAVIGATOR_PRECLAND_H #ifndef NAVIGATOR_PRECLAND_H
#define NAVIGATOR_PRECLAND_H #define NAVIGATOR_PRECLAND_H
#include <controllib/blocks.hpp> #include <matrix/math.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/topics/landing_target_pose.h>
#include <geo/geo.h> #include <geo/geo.h>
#include <px4_module_params.h>
#include <uORB/topics/landing_target_pose.h>
#include "navigator_mode.h" #include "navigator_mode.h"
#include "mission_block.h" #include "mission_block.h"
@@ -64,10 +64,10 @@ enum class PrecLandMode {
Required = 2 // try to find landing target if not visible at the beginning Required = 2 // try to find landing target if not visible at the beginning
}; };
class PrecLand : public MissionBlock class PrecLand : public MissionBlock, public ModuleParams
{ {
public: public:
PrecLand(Navigator *navigator, const char *name); PrecLand(Navigator *navigator);
~PrecLand(); ~PrecLand();
@@ -121,14 +121,16 @@ private:
PrecLandMode _mode; PrecLandMode _mode;
control::BlockParamFloat _param_timeout; DEFINE_PARAMETERS(
control::BlockParamFloat _param_hacc_rad; (ParamFloat<px4::params::PLD_BTOUT>) _param_timeout,
control::BlockParamFloat _param_final_approach_alt; (ParamFloat<px4::params::PLD_HACC_RAD>) _param_hacc_rad,
control::BlockParamFloat _param_search_alt; (ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_final_approach_alt,
control::BlockParamFloat _param_search_timeout; (ParamFloat<px4::params::PLD_SRCH_ALT>) _param_search_alt,
control::BlockParamInt _param_max_searches; (ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_search_timeout,
control::BlockParamFloat _param_acceleration_hor; (ParamInt<px4::params::PLD_MAX_SRCH>) _param_max_searches,
control::BlockParamFloat _param_xy_vel_cruise; (ParamFloat<px4::params::MPC_ACC_HOR>) _param_acceleration_hor,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_xy_vel_cruise
)
}; };
+3 -3
View File
@@ -53,9 +53,9 @@
#include "navigator.h" #include "navigator.h"
#include "datalinkloss.h" #include "datalinkloss.h"
RCLoss::RCLoss(Navigator *navigator, const char *name) : RCLoss::RCLoss(Navigator *navigator) :
MissionBlock(navigator, name), MissionBlock(navigator),
_param_loitertime(this, "LT"), ModuleParams(navigator),
_rcl_state(RCL_STATE_NONE) _rcl_state(RCL_STATE_NONE)
{ {
} }
+7 -4
View File
@@ -40,15 +40,17 @@
#ifndef NAVIGATOR_RCLOSS_H #ifndef NAVIGATOR_RCLOSS_H
#define NAVIGATOR_RCLOSS_H #define NAVIGATOR_RCLOSS_H
#include <px4_module_params.h>
#include "navigator_mode.h" #include "navigator_mode.h"
#include "mission_block.h" #include "mission_block.h"
class Navigator; class Navigator;
class RCLoss final : public MissionBlock class RCLoss : public MissionBlock, public ModuleParams
{ {
public: public:
RCLoss(Navigator *navigator, const char *name); RCLoss(Navigator *navigator);
~RCLoss() = default; ~RCLoss() = default;
void on_inactive() override; void on_inactive() override;
@@ -56,8 +58,9 @@ public:
void on_active() override; void on_active() override;
private: private:
/* Params */ DEFINE_PARAMETERS(
control::BlockParamFloat _param_loitertime; (ParamFloat<px4::params::NAV_RCL_LT>) _param_loitertime
)
enum RCLState { enum RCLState {
RCL_STATE_NONE = 0, RCL_STATE_NONE = 0,
+3 -7
View File
@@ -50,13 +50,9 @@ using math::min;
static constexpr float DELAY_SIGMA = 0.01f; static constexpr float DELAY_SIGMA = 0.01f;
RTL::RTL(Navigator *navigator, const char *name) : RTL::RTL(Navigator *navigator) :
MissionBlock(navigator, name), MissionBlock(navigator),
_param_return_alt(this, "RTL_RETURN_ALT", false), ModuleParams(navigator)
_param_descend_alt(this, "RTL_DESCEND_ALT", false),
_param_land_delay(this, "RTL_LAND_DELAY", false),
_param_rtl_min_dist(this, "RTL_MIN_DIST", false),
_param_rtl_land_type(this, "RTL_LAND_TYPE", false)
{ {
} }
+11 -7
View File
@@ -41,15 +41,17 @@
#ifndef NAVIGATOR_RTL_H #ifndef NAVIGATOR_RTL_H
#define NAVIGATOR_RTL_H #define NAVIGATOR_RTL_H
#include <px4_module_params.h>
#include "navigator_mode.h" #include "navigator_mode.h"
#include "mission_block.h" #include "mission_block.h"
class Navigator; class Navigator;
class RTL final : public MissionBlock class RTL : public MissionBlock, public ModuleParams
{ {
public: public:
RTL(Navigator *navigator, const char *name); RTL(Navigator *navigator);
~RTL() = default; ~RTL() = default;
void on_inactive() override; void on_inactive() override;
@@ -84,11 +86,13 @@ private:
bool _rtl_alt_min{false}; bool _rtl_alt_min{false};
control::BlockParamFloat _param_return_alt; DEFINE_PARAMETERS(
control::BlockParamFloat _param_descend_alt; (ParamFloat<px4::params::RTL_RETURN_ALT>) _param_return_alt,
control::BlockParamFloat _param_land_delay; (ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_descend_alt,
control::BlockParamFloat _param_rtl_min_dist; (ParamFloat<px4::params::RTL_LAND_DELAY>) _param_land_delay,
control::BlockParamInt _param_rtl_land_type; (ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist,
(ParamInt<px4::params::RTL_LAND_TYPE>) _param_rtl_land_type
)
}; };
#endif #endif
+2 -2
View File
@@ -41,8 +41,8 @@
#include "takeoff.h" #include "takeoff.h"
#include "navigator.h" #include "navigator.h"
Takeoff::Takeoff(Navigator *navigator, const char *name) : Takeoff::Takeoff(Navigator *navigator) :
MissionBlock(navigator, name) MissionBlock(navigator)
{ {
} }
+2 -4
View File
@@ -31,7 +31,7 @@
* *
****************************************************************************/ ****************************************************************************/
/** /**
* @file Takeoff.h * @file takeoff.h
* *
* Helper class to take off * Helper class to take off
* *
@@ -41,8 +41,6 @@
#ifndef NAVIGATOR_TAKEOFF_H #ifndef NAVIGATOR_TAKEOFF_H
#define NAVIGATOR_TAKEOFF_H #define NAVIGATOR_TAKEOFF_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include "navigator_mode.h" #include "navigator_mode.h"
#include "mission_block.h" #include "mission_block.h"
@@ -50,7 +48,7 @@
class Takeoff final : public MissionBlock class Takeoff final : public MissionBlock
{ {
public: public:
Takeoff(Navigator *navigator, const char *name); Takeoff(Navigator *navigator);
~Takeoff() = default; ~Takeoff() = default;
void on_activation() override; void on_activation() override;