From c6cdcfc2638f8d994b3716d8739614c5377c4962 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 17:42:21 +0200 Subject: fw pos control: set integrator reset flags in attitude setpoint topic, set them to true when launchdetection is running (while waiting for launch) --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) (limited to 'src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 116d3cc63..518116fa1 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -841,6 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* current waypoint (the one currently heading for) */ math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); + /* Initialize attitude controller integrator reset flags to 0 */ + _att_sp.roll_reset_integral = false; + _att_sp.pitch_reset_integral = false; + _att_sp.yaw_reset_integral = false; /* previous waypoint */ math::Vector<2> prev_wp; @@ -1028,6 +1032,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); last_sent = hrt_absolute_time(); + /* Tell the attitude controller to stop integrating while we are + * waiting for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; } launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { -- cgit v1.2.3 From 819812e11271d7b78f4af2d5bbb7ac6e4be9e494 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 20:52:24 +0200 Subject: fw pos ctrl: move setting of attitude integral reset flag --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) (limited to 'src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 518116fa1..fe5847682 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1032,12 +1032,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); last_sent = hrt_absolute_time(); - /* Tell the attitude controller to stop integrating while we are - * waiting for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; } + + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Detect launch */ launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { launch_detected = true; -- cgit v1.2.3 From 4f560f729ec1f40427401119a0f17d9d0e9843f4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 20:53:06 +0200 Subject: fw pos ctrl: remove comments --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 -- 1 file changed, 2 deletions(-) (limited to 'src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index fe5847682..0e065211e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1024,12 +1024,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ -// warnx("Launch detection running"); if(!launch_detected) { //do not do further checks once a launch was detected if (launchDetector.launchDetectionEnabled()) { static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { -// warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); last_sent = hrt_absolute_time(); } -- cgit v1.2.3