Merge pull request #1073 from PX4/launchdetectorreset

FW: reset attitude control integrals in launchdetection mode (when waiting for takeoff)

3 files changed, 23 insertions, 4 deletions

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 178b590ae..3cd4ce928 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main()
 				float throttle_sp = 0.0f;
 
 				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
+					/* read in attitude setpoint from attitude setpoint uorb topic */
 					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
 					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
-					if (_att_sp.roll_reset_integral)
+					if (_att_sp.roll_reset_integral) {
 						_roll_ctrl.reset_integrator();
-
+					}
+					if (_att_sp.pitch_reset_integral) {
+						_pitch_ctrl.reset_integrator();
+					}
+					if (_att_sp.yaw_reset_integral) {
+						_yaw_ctrl.reset_integrator();
+					}
 				} else {
 					/*
 					 * Scale down roll and pitch as the setpoints are radians
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..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
@@ -841,6 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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;
@@ -1020,15 +1024,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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();
 					}
+
+					/* 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;
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index d526a8ff2..8446e9c6e 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s {
 	float thrust;					/**< Thrust in Newton the power system should generate */
 
 	bool	roll_reset_integral;			/**< Reset roll integral part (navigation logic change) */
+	bool	pitch_reset_integral;			/**< Reset pitch integral part (navigation logic change) */
+	bool	yaw_reset_integral;			/**< Reset yaw integral part (navigation logic change) */
 
 };