Merged beta into master

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -83,6 +83,9 @@
 #include <mavlink/mavlink_log.h>
 #include <platforms/px4_defines.h>
 
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
 #define TILT_COS_MAX	0.7f
 #define SIGMA			0.000001f
 #define MIN_DIST		0.01f
@@ -95,7 +98,7 @@
  */
 extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
 
-class MulticopterPositionControl
+class MulticopterPositionControl : public control::SuperBlock
 {
 public:
 	/**
@@ -122,15 +125,15 @@ private:
 	int		_control_task;			/**< task handle for task */
 	int		_mavlink_fd;			/**< mavlink fd */
 
-	int 	_vehicle_status_sub;	/**< vehicle status subscription */
-	int		_att_sub;				/**< vehicle attitude subscription */
+	int		_vehicle_status_sub;		/**< vehicle status subscription */
+	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_params_sub;			/**< notification of parameter updates */
 	int		_manual_sub;			/**< notification of manual control updates */
 	int		_arming_sub;			/**< arming status of outputs */
 	int		_local_pos_sub;			/**< vehicle local position */
-	int		_pos_sp_triplet_sub;	/**< position setpoint triplet */
+	int		_pos_sp_triplet_sub;		/**< position setpoint triplet */
 	int		_local_pos_sp_sub;		/**< offboard local position setpoint */
 	int		_global_vel_sp_sub;		/**< offboard global velocity setpoint */
 
@@ -138,17 +141,19 @@ private:
 	orb_advert_t	_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
 	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
 
-	struct vehicle_status_s 			_vehicle_status; /**< vehicle status */
+	struct vehicle_status_s 			_vehicle_status; 	/**< vehicle status */
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct vehicle_control_mode_s			_control_mode;	/**< vehicle control mode */
+	struct vehicle_control_mode_s			_control_mode;		/**< vehicle control mode */
 	struct actuator_armed_s				_arming;		/**< actuator arming status */
 	struct vehicle_local_position_s			_local_pos;		/**< vehicle local position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< vehicle global position setpoint triplet */
 	struct vehicle_local_position_setpoint_s	_local_pos_sp;		/**< vehicle local position setpoint */
-	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;	/**< vehicle global velocity setpoint */
+	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;		/**< vehicle global velocity setpoint */
 
+	control::BlockParamFloat _manual_thr_min;
+	control::BlockParamFloat _manual_thr_max;
 
 	struct {
 		param_t thr_min;
@@ -213,7 +218,7 @@ private:
 	/**
 	 * Update our local parameter cache.
 	 */
-	int			parameters_update(bool force);
+	int		parameters_update(bool force);
 
 	/**
 	 * Update control outputs
@@ -293,7 +298,7 @@ MulticopterPositionControl	*g_control;
 }
 
 MulticopterPositionControl::MulticopterPositionControl() :
-
+	SuperBlock(NULL, "MPC"),
 	_task_should_exit(false),
 	_control_task(-1),
 	_mavlink_fd(-1),
@@ -313,7 +318,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_att_sp_pub(nullptr),
 	_local_pos_sp_pub(nullptr),
 	_global_vel_sp_pub(nullptr),
-
+	_manual_thr_min(this, "MANTHR_MIN"),
+	_manual_thr_max(this, "MANTHR_MAX"),
 	_ref_alt(0.0f),
 	_ref_timestamp(0),
 
@@ -413,6 +419,10 @@ MulticopterPositionControl::parameters_update(bool force)
 	}
 
 	if (updated || force) {
+		/* update C++ param system */
+		updateParams();
+
+		/* update legacy C interface params */
 		param_get(_params_handles.thr_min, &_params.thr_min);
 		param_get(_params_handles.thr_max, &_params.thr_max);
 		param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
@@ -1416,11 +1426,11 @@ MulticopterPositionControl::task_main()
 
 			/* control throttle directly if no climb rate controller is active */
 			if (!_control_mode.flag_control_climb_rate_enabled) {
-				_att_sp.thrust = math::min(_manual.z, _params.thr_max);
+				_att_sp.thrust = math::min(_manual.z, _manual_thr_max.get());
 
 				/* enforce minimum throttle if not landed */
 				if (!_vehicle_status.condition_landed) {
-					_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min);
+					_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
 				}
 			}
 

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -41,7 +41,32 @@
 #include <systemlib/param/param.h>
 
 /**
- * Minimum thrust
+ * Minimum thrust in auto thrust control
+ *
+ * It's recommended to set it > 0 to avoid free fall with zero thrust.
+ *
+ * @min 0.05
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
+
+/**
+ * Maximum thrust in auto thrust control
+ *
+ * Limit max allowed thrust. Setting a value of one can put
+ * the system into actuator saturation as no spread between
+ * the motors is possible any more. A value of 0.8 - 0.9
+ * is recommended.
+ *
+ * @min 0.0
+ * @max 0.95
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
+
+/**
+ * Minimum manual thrust
  *
  * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
  *
@@ -49,10 +74,10 @@
  * @max 1.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
+PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.12f);
 
 /**
- * Maximum thrust
+ * Maximum manual thrust
  *
  * Limit max allowed thrust. Setting a value of one can put
  * the system into actuator saturation as no spread between
@@ -63,7 +88,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
  * @max 1.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
+PARAM_DEFINE_FLOAT(MPC_MANTHR_MAX, 0.9f);
 
 /**
  * Proportional gain for vertical position error