multirotor_pos_control fixes, introduced HARD control mode (disabled by default)

3 files changed, 49 insertions, 15 deletions

diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index ad5670edc..c94972e7d 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -283,11 +283,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 			float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max;
 			float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max;
 
+			float pos_sp_speed_x = 0.0f;
+			float pos_sp_speed_y = 0.0f;
+			float pos_sp_speed_z = 0.0f;
+
 			if (status.flag_control_manual_enabled) {
 				/* move altitude setpoint with manual controls */
 				float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
 				if (alt_sp_ctl != 0.0f) {
-					local_pos_sp.z -= alt_sp_ctl * params.alt_rate_max * dt;
+					pos_sp_speed_z = -alt_sp_ctl * params.alt_rate_max;
+					local_pos_sp.z += pos_sp_speed_z * dt;
 					if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) {
 						local_pos_sp.z = local_pos.z + alt_err_linear_limit;
 					} else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) {
@@ -297,14 +302,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 
 				if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) {
 					/* move position setpoint with manual controls */
-					float pos_x_sp_ctl = scale_control(-manual.pitch, 1.0f, pos_ctl_dz);
-					float pos_y_sp_ctl = scale_control(manual.roll, 1.0f, pos_ctl_dz);
+					float pos_x_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
+					float pos_y_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
 					if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) {
 						/* calculate direction and increment of control in NED frame */
 						float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl);
-						float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max * dt;
-						local_pos_sp.x += cosf(dir_sp_ctl) * d_sp_ctl;
-						local_pos_sp.y += sinf(dir_sp_ctl) * d_sp_ctl;
+						float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max;
+						pos_sp_speed_x = cosf(dir_sp_ctl) * d_sp_ctl;
+						pos_sp_speed_x = sinf(dir_sp_ctl) * d_sp_ctl;
+						local_pos_sp.x += pos_sp_speed_x * dt;
+						local_pos_sp.y += pos_sp_speed_y * dt;
 						/* limit maximum setpoint from position offset and preserve direction */
 						float pos_vec_x = local_pos_sp.x - local_pos.x;
 						float pos_vec_y = local_pos_sp.y - local_pos.y;
@@ -315,15 +322,19 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 						}
 					}
 				}
+
+				if (params.hard == 0) {
+					pos_sp_speed_x = 0.0f;
+					pos_sp_speed_y = 0.0f;
+					pos_sp_speed_z = 0.0f;
+				}
 			}
 
 			/* PID for altitude */
 			/* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */
-			float alt_err = limit_value(local_pos.z - local_pos_sp.z, alt_err_linear_limit);
+			float alt_err = limit_value(local_pos_sp.z - local_pos.z, alt_err_linear_limit);
 			/* P and D components */
-			float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d;
-			/* add I component */
-			float thrust_ctl = thrust_ctl_pd + alt_integral;
+			float thrust_ctl_pd = -(alt_err * params.alt_p + (pos_sp_speed_z - local_pos.vz) * params.alt_d);	// altitude = -z
 			/* integrate */
 			alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt;
 			if (alt_integral < params.thr_min) {
@@ -331,6 +342,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 			} else if (alt_integral > params.thr_max) {
 				alt_integral = params.thr_max;
 			}
+			/* add I component */
+			float thrust_ctl = thrust_ctl_pd + alt_integral;
 			if (thrust_ctl < params.thr_min) {
 				thrust_ctl = params.thr_min;
 			} else if (thrust_ctl > params.thr_max) {
@@ -342,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 				float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit);
 				float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit);
 				/* P and D components */
-				float pos_x_ctl_pd = - pos_x_err * params.pos_p - local_pos.vx * params.pos_d;
-				float pos_y_ctl_pd = - pos_y_err * params.pos_p - local_pos.vy * params.pos_d;
-				/* add I component */
-				float pos_x_ctl = pos_x_ctl_pd + pos_x_integral;
-				float pos_y_ctl = pos_y_ctl_pd + pos_y_integral;
+				float pos_x_ctl_pd = - pos_x_err * params.pos_p + (pos_sp_speed_x - local_pos.vx) * params.pos_d;
+				float pos_y_ctl_pd = - pos_y_err * params.pos_p + (pos_sp_speed_y - local_pos.vy) * params.pos_d;
 				/* integrate */
 				pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max);
 				pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max);
+				/* add I component */
+				float pos_x_ctl = pos_x_ctl_pd + pos_x_integral;
+				float pos_y_ctl = pos_y_ctl_pd + pos_y_integral;
 				/* calculate direction and slope in NED frame */
 				float dir = atan2f(pos_y_ctl, pos_x_ctl);
 				/* use approximation: slope ~ sin(slope) = force */
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index 7f2ba3db8..7c3296cae 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -53,6 +53,7 @@ PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f);
 PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 10.0f);
 PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f);
+PARAM_DEFINE_INT32(MPC_HARD, 0);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)
 {
@@ -67,6 +68,11 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
 	h->pos_d 	=	param_find("MPC_POS_D");
 	h->pos_rate_max 	=	param_find("MPC_POS_RATE_MAX");
 	h->slope_max 	=	param_find("MPC_SLOPE_MAX");
+	h->slope_max 	=	param_find("MPC_HARD");
+
+	h->rc_scale_pitch    =   param_find("RC_SCALE_PITCH");
+	h->rc_scale_roll    =   param_find("RC_SCALE_ROLL");
+	h->rc_scale_yaw      =   param_find("RC_SCALE_YAW");
 
 	return OK;
 }
@@ -84,6 +90,11 @@ int parameters_update(const struct multirotor_position_control_param_handles *h,
 	param_get(h->pos_d, &(p->pos_d));
 	param_get(h->pos_rate_max, &(p->pos_rate_max));
 	param_get(h->slope_max, &(p->slope_max));
+	param_get(h->hard, &(p->hard));
+
+	param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
+	param_get(h->rc_scale_roll, &(p->rc_scale_roll));
+	param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
 
 	return OK;
 }
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
index 589ee92a1..13b667ad3 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -53,6 +53,11 @@ struct multirotor_position_control_params {
 	float pos_d;
 	float pos_rate_max;
 	float slope_max;
+	int hard;
+
+	float rc_scale_pitch;
+	float rc_scale_roll;
+	float rc_scale_yaw;
 };
 
 struct multirotor_position_control_param_handles {
@@ -67,6 +72,11 @@ struct multirotor_position_control_param_handles {
 	param_t pos_d;
 	param_t pos_rate_max;
 	param_t slope_max;
+	param_t hard;
+
+	param_t rc_scale_pitch;
+	param_t rc_scale_roll;
+	param_t rc_scale_yaw;
 };
 
 /**