LP-603 Make QUATERNION_STABILIZATION optional. It can be harmful when using POIs on multicopter and the desired yaw jumps a lot, suddenly

Author: AIRCAP

flight/modules/Stabilization/outerloop.c

@@ -28,7 +28,7 @@
  * for more details.
  *
  * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
+ * with this program; if not, write to the Free Software Foundation, In.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
@@ -146,8 +146,8 @@ static void stabilizationOuterloopTask()
 
 
     float local_error[3];
-    {
 #if defined(PIOS_QUATERNION_STABILIZATION)
+    if (stabSettings.settings.ForceRollPitchDuringYawTransition == STABILIZATIONSETTINGS_FORCEROLLPITCHDURINGYAWTRANSITION_FALSE) {
         // Quaternion calculation of error in each axis.  Uses more memory.
         float rpy_desired[3];
         float q_desired[4];
@@ -173,8 +173,10 @@ static void stabilizationOuterloopTask()
         quat_mult(q_desired, &attitudeState.q1, q_error);
         quat_inverse(q_error);
         Quaternion2RPY(q_error, local_error);
-
+    } else {
 #else /* if defined(PIOS_QUATERNION_STABILIZATION) */
+    {
+#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
         // Simpler algorithm for CC, less memory
         local_error[0] = stabilizationDesiredAxis[0] - attitudeState.Roll;
         local_error[1] = stabilizationDesiredAxis[1] - attitudeState.Pitch;
@@ -187,7 +189,6 @@ static void stabilizationOuterloopTask()
         } else {
             local_error[2] = modulo - 180.0f;
         }
-#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
     }
 
 

shared/uavobjectdefinition/stabilizationsettings.xml

@@ -49,6 +49,7 @@
 	<field name="FlightModeAssistMap" units="" type="enum" options="None,GPSAssist" elements="6" defaultvalue="None,None,None,None,None,None" />
 
 	<field name="MeasureBasedDTerm" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
+	<field name="ForceRollPitchDuringYawTransition" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
 
 	<access gcs="readwrite" flight="readwrite"/>
 	<telemetrygcs acked="true" updatemode="onchange" period="0"/>