AP_Math: added O3 optimisation to core math libraries

this costs some flash space but speeds things up considerably

libraries/AP_Math/matrix3.cpp

@@ -17,6 +17,8 @@
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#pragma GCC optimize("O3")
+
 #include "AP_Math.h"
 
 // create a rotation matrix given some euler angles

libraries/AP_Math/matrix_alg.cpp

@@ -1,3 +1,5 @@
+#pragma GCC optimize("O3")
+
 #include <AP_Math/AP_Math.h>
 #include <AP_HAL/AP_HAL.h>
 

libraries/AP_Math/quaternion.cpp

@@ -17,6 +17,8 @@
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#pragma GCC optimize("O3")
+
 #include "AP_Math.h"
 
 // return the rotation matrix equivalent for this quaternion

libraries/AP_Math/vector2.cpp

@@ -17,6 +17,8 @@
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#pragma GCC optimize("O3")
+
 #include "AP_Math.h"
 
 template <typename T>

libraries/AP_Math/vector3.cpp

@@ -17,6 +17,8 @@
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#pragma GCC optimize("O3")
+
 #include "AP_Math.h"
 
 #define HALF_SQRT_2 0.70710678118654757f