multirotor_pos_control: tunable velocity feed forward for SEATBELT/EASY modes

3 files changed, 13 insertions, 3 deletions

diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 54f9e52b8..fe9a292b0 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -612,7 +612,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];
+				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;
 
 			} else {
 				reset_man_sp_z = true;
@@ -621,8 +621,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];
-				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];
+				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 */
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 cfb5dc0d4..1c798b007 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -49,12 +49,14 @@ 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);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)
@@ -69,12 +71,14 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
 	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");
 
 	h->rc_scale_pitch    =   param_find("RC_SCALE_PITCH");
@@ -96,12 +100,14 @@ int parameters_update(const struct multirotor_position_control_param_handles *h,
 	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));
 
 	param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
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 fc658dadb..804716d11 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -51,12 +51,14 @@ struct multirotor_position_control_params {
 	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;
 
 	float rc_scale_pitch;
@@ -75,12 +77,14 @@ struct multirotor_position_control_param_handles {
 	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;
 
 	param_t rc_scale_pitch;