Fix automatic yaw to north at takeoff with PID controller.

mpaperno · Apr 15, 2016 · 0ad2825 · 0ad2825
1 parent 6110b9b
commit 0ad2825
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 6 deletions.
diff --git a/CHANGES.md b/CHANGES.md
@@ -10,6 +10,10 @@ This file describes changes in the firmware, typically since the previous minor
 	! : important change, possible danger, change of default behavior, etc.
 
 
+##### 7.1.1919 - Apr. 15, 2016 - `feature_integration` branch
+
+`*` Fix automatic yaw to north at takeoff with PID controller.  
+
 ##### 7.1.1918 - Apr. 14, 2016 - `feature_integration` branch
 
 `~` Aggregate changes from `new_controls`, `sport_telemetry`, `memory_manage`, `simu` branches.  

diff --git a/src/buildnum.h b/src/buildnum.h
@@ -1 +1 @@
-#define BUILDNUMBER 1918
+#define BUILDNUMBER 1919
diff --git a/src/control.c b/src/control.c
@@ -134,16 +134,13 @@ void controlTaskCode(void *unused) {
 
 		// motors not running yet,
 		if (motorsData.throttle == 0) {
-		    // if motors are not yet running, use this heading as hold heading
-		    controlData.anglesDesired[RPY_Y] = navData.holdHeading = AQ_YAW;
-
 		    // Reset all PIDs and filters
 		    for (axis = 0; axis < 3; ++axis) {
 			overrides[axis] = 0;
 			utilFilterReset3(controlData.rateFilter[axis], 0.0f);
 			if (controlData.controllerType == CONTROLLER_TYPE_QUATOS)
 			    continue;
-			utilFilterReset3(controlData.angleFilter[axis], 0.0f);
+			utilFilterReset3(controlData.angleFilter[axis], (axis == RPY_Y ? AQ_YAW : 0.0f));
 			pidZeroIntegral(controlData.ratePID[axis], 0.0f, 0.0f);
 			pidZeroIntegral(controlData.anglePID[axis], 0.0f, 0.0f);
 			pidZeroIntegral(controlData.rateModePID[axis], 0.0f, 0.0f);
@@ -157,6 +154,9 @@ void controlTaskCode(void *unused) {
 		    }
 #endif
 
+		    // use this heading as hold heading
+		    controlData.anglesDesired[RPY_Y] = navData.holdHeading = AQ_YAW;
+
 		    // also set this position as hold position
 		    if (navData.mode == NAV_STATUS_POSHOLD)
 			navUkfSetHereAsPositionTarget();
@@ -357,7 +357,7 @@ void controlTaskCode(void *unused) {
 #ifdef HAS_QUATOS
 		// Quatos
 		else {
-		    // rates mode or startup
+		    // rates mode
 		    if (controlData.controlMode > CTRL_MODE_ANGLE && !rateTiltLimit) {
 			// keep up with current orientation
 			quatos(&UKF_Q1, &UKF_Q1, ratesActual, controlData.ratesDesired, overrides);