diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-03 07:50:05 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-03 07:50:05 +0200 |
commit | 238f018090668226e7afe916b29a736760dfcf04 (patch) | |
tree | f558d8edad9fd3890db5de9662c307c2fd82f56d /src | |
parent | 3c0c11aec3c73d579659d81f9b68efbf7425846d (diff) | |
parent | 29c06e39431eb6e5112a2cefa9969d59fa2ff875 (diff) | |
download | px4-firmware-238f018090668226e7afe916b29a736760dfcf04.tar.gz px4-firmware-238f018090668226e7afe916b29a736760dfcf04.tar.bz2 px4-firmware-238f018090668226e7afe916b29a736760dfcf04.zip |
Merge branch 'master' of github.com:PX4/Firmware
Diffstat (limited to 'src')
5 files changed, 18 insertions, 8 deletions
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f22dac0c1..40d0386d5 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -46,6 +46,7 @@ #include <systemlib/param/param.h> PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f); PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a25448af2..336523072 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(¶ms_h); parameters_update(¶ms_h, ¶ms); + 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); @@ -350,7 +351,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } /* move altitude setpoint with throttle stick */ @@ -423,12 +424,12 @@ 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 - params.takeoff_gap; local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { @@ -504,7 +505,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* set altitude setpoint to 5m under ground, * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); } reset_sp_z = true; @@ -514,7 +515,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); } } 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..bf9578ea3 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,8 @@ 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->takeoff_gap = param_find("NAV_TAKEOFF_GAP"); 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 +86,8 @@ 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->takeoff_gap, &(p->takeoff_gap)); 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..fc658dadb 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,8 @@ #include <systemlib/param/param.h> struct multirotor_position_control_params { + float takeoff_alt; + float takeoff_gap; float thr_min; float thr_max; float z_p; @@ -63,6 +65,8 @@ struct multirotor_position_control_params { }; struct multirotor_position_control_param_handles { + param_t takeoff_alt; + param_t takeoff_gap; param_t thr_min; param_t thr_max; param_t z_p; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index deed25836..0edd91b24 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -358,8 +358,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while outputs armed */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + /* do not allow a mixer change while safety off */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { return; } |