1 files changed, 46 insertions, 8 deletions

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 6b73dc405..7c3296cae 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -34,29 +34,67 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_position_control_params.c
+ * @file multirotor_pos_control_params.c
  * 
- * Parameters for EKF filter
+ * Parameters for multirotor_pos_control
  */
 
 #include "multirotor_pos_control_params.h"
 
-/* Extended Kalman Filter covariances */
-
 /* controller parameters */
-PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f);
+PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f);
+PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f);
+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)
 {
-	/* PID parameters */
-	h->p 	=	param_find("MC_POS_P");
+	h->thr_min 	=	param_find("MPC_THR_MIN");
+	h->thr_max 	=	param_find("MPC_THR_MAX");
+	h->alt_p 	=	param_find("MPC_ALT_P");
+	h->alt_i 	=	param_find("MPC_ALT_I");
+	h->alt_d 	=	param_find("MPC_ALT_D");
+	h->alt_rate_max 	=	param_find("MPC_ALT_RATE_MAX");
+	h->pos_p 	=	param_find("MPC_POS_P");
+	h->pos_i 	=	param_find("MPC_POS_I");
+	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;
 }
 
 int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
 {
-	param_get(h->p, &(p->p));
+	param_get(h->thr_min, &(p->thr_min));
+	param_get(h->thr_max, &(p->thr_max));
+	param_get(h->alt_p, &(p->alt_p));
+	param_get(h->alt_i, &(p->alt_i));
+	param_get(h->alt_d, &(p->alt_d));
+	param_get(h->alt_rate_max, &(p->alt_rate_max));
+	param_get(h->pos_p, &(p->pos_p));
+	param_get(h->pos_i, &(p->pos_i));
+	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;
 }