[WIP] adaptive hover-throttle for altitude control

msg/vehicle_local_position_setpoint.msg

@@ -18,5 +18,6 @@ float32 jerk_x		# in meters/(sec*sec*sec)
 float32 jerk_y		# in meters/(sec*sec*sec)
 float32 jerk_z		# in meters/(sec*sec*sec)
 float32[3] thrust	# normalized thrust vector in NED
+float32 hover_estimation
 
 # TOPICS vehicle_local_position_setpoint trajectory_setpoint

src/modules/mc_pos_control/PositionControl.cpp

@@ -45,7 +45,7 @@ using namespace matrix;
 
 PositionControl::PositionControl(ModuleParams *parent) :
 	ModuleParams(parent)
-{}
+{_hover_est = _param_mpc_thr_hover.get();}
 
 void PositionControl::updateState(const PositionControlStates &states)
 {
@@ -256,10 +256,42 @@ void PositionControl::_velocityController(const float &dt)
 	// 	 NE-direction is also limited by the maximum tilt.
 
 	const Vector3f vel_err = _vel_sp - _vel;
+	float z_err = 0.0;
+
+	if (_param_mpc_hov_est_en.get()) {
+		float k1 = _param_mpc_hov_est_p.get();
+		float est_gain  = -_param_mpc_hov_adt_rate.get();
+
+		z_err = _pos_sp(2) - _pos(2);
+
+		printf("Zerr %.5f - vel_err %.5f \n", (double)z_err, (double)vel_err(2));
+
+		float p = vel_err(2) + k1 * z_err;
+		printf("p = %.2f \n", (double) p);
+
+		if (_hover_est >= 1.0f && p <= 0) {
+			printf("Max hover \n");
+			p = 0;
+		}
+
+		if (_hover_est <= 0.0f && p >= 0) {
+			p = 0;
+			printf("Min hover \n");
+		}
+
+
+		_hover_est += est_gain * p * dt;
+
+		printf("Hover est %.2f \n", (double) _hover_est);
+
+	} else {_hover_est = _param_mpc_thr_hover.get();}
 
 	// Consider thrust in D-direction.
+	// float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) +  _param_mpc_z_vel_d.get() * _vel_dot(2) + _thr_int(
+	//				 2) - _param_mpc_thr_hover.get();
+
 	float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) +  _param_mpc_z_vel_d.get() * _vel_dot(2) + _thr_int(
-					 2) - _param_mpc_thr_hover.get();
+					 2) - _hover_est;
 
 	// The Thrust limits are negated and swapped due to NED-frame.
 	float uMax = -_param_mpc_thr_min.get();
@@ -355,3 +387,9 @@ void PositionControl::updateParams()
 {
 	ModuleParams::updateParams();
 }
+
+
+void PositionControl::setHoverEst()
+{
+	_hover_est = _param_mpc_thr_hover.get();
+}

src/modules/mc_pos_control/PositionControl.hpp

@@ -149,6 +149,9 @@ class PositionControl : public ModuleParams
 	 */
 	const float &getYawspeedSetpoint() { return _yawspeed_sp; }
 
+	const float &getHoverEst() { return _hover_est; }
+	void  setHoverEst();
+
 	/**
 	 * 	Get the
 	 * 	@see _vel_sp
@@ -191,6 +194,7 @@ class PositionControl : public ModuleParams
 		return pos_sp;
 	}
 
+
 protected:
 
 	void updateParams() override;
@@ -222,7 +226,7 @@ class PositionControl : public ModuleParams
 	bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */
 	bool _ctrl_pos[3] = {true, true, true}; /**< True if the control-loop for position was used */
 	bool _ctrl_vel[3] = {true, true, true}; /**< True if the control-loop for velocity was used */
-
+	float _hover_est = 0;
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max,
 		(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
@@ -242,6 +246,11 @@ class PositionControl : public ModuleParams
 		(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
 		(ParamFloat<px4::params::MPC_XY_VEL_P>) _param_mpc_xy_vel_p,
 		(ParamFloat<px4::params::MPC_XY_VEL_I>) _param_mpc_xy_vel_i,
-		(ParamFloat<px4::params::MPC_XY_VEL_D>) _param_mpc_xy_vel_d
+		(ParamFloat<px4::params::MPC_XY_VEL_D>) _param_mpc_xy_vel_d,
+		(ParamFloat<px4::params::MPC_HOV_ADT_RATE>) _param_mpc_hov_adt_rate,
+		(ParamFloat<px4::params::MPC_HOV_EST_P>) _param_mpc_hov_est_p,
+		(ParamInt<px4::params::MPC_HOV_EST_EN>) _param_mpc_hov_est_en
+
+
 	)
 };

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -616,6 +616,7 @@ MulticopterPositionControl::run()
 				// prevent any integrator windup
 				_control.resetIntegralXY();
 				_control.resetIntegralZ();
+				_control.setHoverEst();
 				// reactivate the task which will reset the setpoint to current state
 				_flight_tasks.reActivate();
 			}
@@ -661,8 +662,10 @@ MulticopterPositionControl::run()
 			local_pos_sp.vx = PX4_ISFINITE(_control.getVelSp()(0)) ? _control.getVelSp()(0) : setpoint.vx;
 			local_pos_sp.vy = PX4_ISFINITE(_control.getVelSp()(1)) ? _control.getVelSp()(1) : setpoint.vy;
 			local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz;
+			local_pos_sp.hover_estimation = _control.getHoverEst();
 			_control.getThrustSetpoint().copyTo(local_pos_sp.thrust);
 
+
 			// Publish local position setpoint
 			// This message will be used by other modules (such as Landdetector) to determine
 			// vehicle intention.

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -771,3 +771,8 @@ PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f);
  * @group Mission
  */
 PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);
+
+
+PARAM_DEFINE_FLOAT(MPC_HOV_ADT_RATE, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_HOV_EST_P, 0.0f);
+PARAM_DEFINE_INT32(MPC_HOV_EST_EN, 0);