From 2c8c6efb457cc5b062f467ce0f7e49cde7c8000a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 8 Feb 2016 10:58:25 +1100 Subject: [PATCH] EKF: Eliminate use of power function to square numbers Use of the power function to square a number can be very inefficient depending on processor and compiler. Replace with an inlined function that multiplies the number by itself. This has not been put into the maths library because the use of the sq() function is peculiar to the autocode generated for the EKF and an inlined function was desired. --- EKF/covariance.cpp | 2 -- EKF/ekf.h | 8 ++++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index ca491ced0a..105a692602 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -44,8 +44,6 @@ #include #include -#define sq(_arg) powf(_arg, 2.0f) - void Ekf::initialiseCovariance() { for (unsigned i = 0; i < _k_num_states; i++) { diff --git a/EKF/ekf.h b/EKF/ekf.h index 726d1ffa60..9b7ebc285f 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -42,8 +42,6 @@ #include "estimator_interface.h" -#define sq(_arg) powf(_arg, 2.0f) - class Ekf : public EstimatorInterface { public: @@ -202,4 +200,10 @@ private: // Determine if we are airborne or motors are armed void calculateVehicleStatus(); + + // return the square of two foating point numbers - used in autocoded sections + inline float sq(float var) + { + return var * var; + } };