multirotor_pos_control: remove unused parameters, use ellipsoid for velocity setpoint limiting

3 files changed, 20 insertions, 49 deletions

diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 74ae5947f..378d3fda7 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -173,6 +173,11 @@ static float norm(float x, float y)
 	return sqrtf(x * x + y * y);
 }
 
+static float norm3(float x, float y, float z)
+{
+	return sqrtf(x * x + y * y + z * z);
+}
+
 static void cross3(float a[3], float b[3], float res[3])
 {
 	res[0] = a[1] * b[2] - a[2] * b[1];
@@ -298,8 +303,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	hrt_abstime ref_alt_t = 0;
 	uint64_t local_ref_timestamp = 0;
 
-	PID_t xy_pos_pids[2];
-	PID_t z_pos_pid;
 	float thrust_int[3] = { 0.0f, 0.0f, 0.0f };
 
 	thread_running = true;
@@ -309,12 +312,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	parameters_init(&params_h);
 	parameters_update(&params_h, &params);
 
-	for (int i = 0; i < 2; i++) {
-		pid_init(&(xy_pos_pids[i]), PID_MODE_DERIVATIV_SET, 0.02f);
-	}
-
-	pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f);
-
 	bool param_updated = true;
 
 	while (!thread_should_exit) {
@@ -329,12 +326,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 			/* update params */
 			parameters_update(&params_h, &params);
-
-			for (int i = 0; i < 2; i++) {
-				pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 0.0f, 0.0f);
-			}
-
-			pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
 		}
 
 		bool updated;
@@ -362,6 +353,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 		} else {
 			dt = 0.0f;
 		}
+		t_prev = t;
 
 		if (control_mode.flag_armed && !was_armed) {
 			/* reset setpoints and integrals on arming */
@@ -376,8 +368,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 		was_armed = control_mode.flag_armed;
 
-		t_prev = t;
-
 		if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_position_enabled || control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
 			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
 			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
@@ -612,7 +602,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 			/* run position & altitude controllers, calculate velocity setpoint */
 			if (control_mode.flag_control_altitude_enabled) {
-				global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2] * params.z_ff;
+				global_vel_sp.vz = (local_pos_sp.z - local_pos.z) * params.z_p + sp_move_rate[2] * params.z_ff;
 
 			} else {
 				reset_man_sp_z = true;
@@ -621,18 +611,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 			if (control_mode.flag_control_position_enabled) {
 				/* calculate velocity set point in NED frame */
-				global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0] * params.xy_ff;
-				global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1] * params.xy_ff;
-
-				if (!control_mode.flag_control_manual_enabled) {
-					/* limit horizontal speed only in AUTO mode */
-					float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
-
-					if (xy_vel_sp_norm > 1.0f) {
-						global_vel_sp.vx /= xy_vel_sp_norm;
-						global_vel_sp.vy /= xy_vel_sp_norm;
-					}
-				}
+				global_vel_sp.vx = (local_pos_sp.x - local_pos.x) * params.xy_p + sp_move_rate[0] * params.xy_ff;
+				global_vel_sp.vy = (local_pos_sp.y - local_pos.y) * params.xy_p + sp_move_rate[1] * params.xy_ff;
 
 			} else {
 				reset_man_sp_xy = true;
@@ -640,6 +620,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				global_vel_sp.vy = 0.0f;
 			}
 
+			if (!control_mode.flag_control_manual_enabled) {
+				/* limit 3D speed only in AUTO mode */
+				float vel_sp_norm = norm3(global_vel_sp.vx / params.xy_vel_max, global_vel_sp.vy / params.xy_vel_max, global_vel_sp.vz / params.z_vel_max);
+
+				if (vel_sp_norm > 1.0f) {
+					global_vel_sp.vx /= vel_sp_norm;
+					global_vel_sp.vy /= vel_sp_norm;
+					global_vel_sp.vz /= vel_sp_norm;
+				}
+			}
+
 			/* publish new velocity setpoint */
 			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
 			// TODO subscribe to velocity setpoint if altitude/position control disabled
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 1c798b007..bebcdb3f5 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -44,17 +44,13 @@
 PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
 PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
 PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
-PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f);
 PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f);
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
 PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
 PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
-PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f);
-PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
 PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
 PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
@@ -66,17 +62,13 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
 	h->thr_min 	=	param_find("MPC_THR_MIN");
 	h->thr_max 	=	param_find("MPC_THR_MAX");
 	h->z_p 	=	param_find("MPC_Z_P");
-	h->z_d 	=	param_find("MPC_Z_D");
 	h->z_vel_p 	=	param_find("MPC_Z_VEL_P");
 	h->z_vel_i 	=	param_find("MPC_Z_VEL_I");
-	h->z_vel_d 	=	param_find("MPC_Z_VEL_D");
 	h->z_vel_max 	=	param_find("MPC_Z_VEL_MAX");
 	h->z_ff 	=	param_find("MPC_Z_FF");
 	h->xy_p 	=	param_find("MPC_XY_P");
-	h->xy_d 	=	param_find("MPC_XY_D");
 	h->xy_vel_p 	=	param_find("MPC_XY_VEL_P");
 	h->xy_vel_i 	=	param_find("MPC_XY_VEL_I");
-	h->xy_vel_d 	=	param_find("MPC_XY_VEL_D");
 	h->xy_vel_max 	=	param_find("MPC_XY_VEL_MAX");
 	h->xy_ff 	=	param_find("MPC_XY_FF");
 	h->tilt_max 	=	param_find("MPC_TILT_MAX");
@@ -95,17 +87,13 @@ int parameters_update(const struct multirotor_position_control_param_handles *h,
 	param_get(h->thr_min, &(p->thr_min));
 	param_get(h->thr_max, &(p->thr_max));
 	param_get(h->z_p, &(p->z_p));
-	param_get(h->z_d, &(p->z_d));
 	param_get(h->z_vel_p, &(p->z_vel_p));
 	param_get(h->z_vel_i, &(p->z_vel_i));
-	param_get(h->z_vel_d, &(p->z_vel_d));
 	param_get(h->z_vel_max, &(p->z_vel_max));
 	param_get(h->z_ff, &(p->z_ff));
 	param_get(h->xy_p, &(p->xy_p));
-	param_get(h->xy_d, &(p->xy_d));
 	param_get(h->xy_vel_p, &(p->xy_vel_p));
 	param_get(h->xy_vel_i, &(p->xy_vel_i));
-	param_get(h->xy_vel_d, &(p->xy_vel_d));
 	param_get(h->xy_vel_max, &(p->xy_vel_max));
 	param_get(h->xy_ff, &(p->xy_ff));
 	param_get(h->tilt_max, &(p->tilt_max));
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 804716d11..37a925dcc 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -46,17 +46,13 @@ struct multirotor_position_control_params {
 	float thr_min;
 	float thr_max;
 	float z_p;
-	float z_d;
 	float z_vel_p;
 	float z_vel_i;
-	float z_vel_d;
 	float z_vel_max;
 	float z_ff;
 	float xy_p;
-	float xy_d;
 	float xy_vel_p;
 	float xy_vel_i;
-	float xy_vel_d;
 	float xy_vel_max;
 	float xy_ff;
 	float tilt_max;
@@ -72,17 +68,13 @@ struct multirotor_position_control_param_handles {
 	param_t thr_min;
 	param_t thr_max;
 	param_t z_p;
-	param_t z_d;
 	param_t z_vel_p;
 	param_t z_vel_i;
-	param_t z_vel_d;
 	param_t z_vel_max;
 	param_t z_ff;
 	param_t xy_p;
-	param_t xy_d;
 	param_t xy_vel_p;
 	param_t xy_vel_i;
-	param_t xy_vel_d;
 	param_t xy_vel_max;
 	param_t xy_ff;
 	param_t tilt_max;