dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

respect NAV_TAKEOFF_ALT instead of using hardcoded default takeoff value

This commit is contained in:
tstellanova 2013-09-02 09:13:36 -07:00
parent 38f9c25e93
commit 027d45acbf
3 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -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;

2
src/modules/multirotor_pos_control/multirotor_pos_control_params.c
View File

@ -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));

2
src/modules/multirotor_pos_control/multirotor_pos_control_params.h
View File

@ -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;