respect NAV_TAKEOFF_ALT instead of using hardcoded default takeoff value

3 files changed, 7 insertions, 2 deletions

diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index a25448af2..c194f627d 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -227,7 +227,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	hrt_abstime t_prev = 0;
 	const float alt_ctl_dz = 0.2f;
 	const float pos_ctl_dz = 0.05f;
-	const float takeoff_alt_default = 10.0f;
+
 	float ref_alt = 0.0f;
 	hrt_abstime ref_alt_t = 0;
 	uint64_t local_ref_timestamp = 0;
@@ -244,6 +244,7 @@ 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]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
 		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
@@ -423,7 +424,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					if (reset_auto_pos) {
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
-						local_pos_sp.z = -takeoff_alt_default;
+						local_pos_sp.z = -params.takeoff_alt;
 						local_pos_sp.yaw = att.yaw;
 						local_pos_sp_valid = true;
 						att_sp.yaw_body = att.yaw;
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 9c1ef2edb..4d6511851 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -59,6 +59,7 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)
 {
+	h->takeoff_alt = param_find("NAV_TAKEOFF_ALT");
 	h->thr_min 	=	param_find("MPC_THR_MIN");
 	h->thr_max 	=	param_find("MPC_THR_MAX");
 	h->z_p 	=	param_find("MPC_Z_P");
@@ -84,6 +85,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
 
 int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
 {
+	param_get(h->takeoff_alt,&(p->takeoff_alt));
 	param_get(h->thr_min, &(p->thr_min));
 	param_get(h->thr_max, &(p->thr_max));
 	param_get(h->z_p, &(p->z_p));
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 3ec85a364..79bc9b72b 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -41,6 +41,7 @@
 #include <systemlib/param/param.h>
 
 struct multirotor_position_control_params {
+    float takeoff_alt;
 	float thr_min;
 	float thr_max;
 	float z_p;
@@ -63,6 +64,7 @@ struct multirotor_position_control_params {
 };
 
 struct multirotor_position_control_param_handles {
+    param_t takeoff_alt;
 	param_t thr_min;
 	param_t thr_max;
 	param_t z_p;