Line data    Source code

       1             : /*
       2             :   NavEKF_core_common holds scratch data shared by EKF2 and EKF3
       3             : 
       4             :   This program is free software: you can redistribute it and/or modify
       5             :   it under the terms of the GNU General Public License as published by
       6             :   the Free Software Foundation, either version 3 of the License, or
       7             :   (at your option) any later version.
       8             : 
       9             :   This program is distributed in the hope that it will be useful,
      10             :   but WITHOUT ANY WARRANTY; without even the implied warranty of
      11             :   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      12             :   GNU General Public License for more details.
      13             : 
      14             :   You should have received a copy of the GNU General Public License
      15             :   along with this program.  If not, see <http://www.gnu.org/licenses/>.
      16             :  */
      17             : #pragma once
      18             : 
      19             : #include <stdint.h>
      20             : #include <AP_Math/AP_Math.h>
      21             : #include <AP_Math/vectorN.h>
      22             : #include "AP_Nav_Common.h"
      23             : 
      24             : /*
      25             :   this declares a common parent class for AP_NavEKF2 and
      26             :   AP_NavEKF3. The purpose of this class is to hold common static
      27             :   scratch space variables. These variables do not hold anything that
      28             :   matters between iterations, they are only intermediate variables. By
      29             :   placing these in a common parent class we save a lot of memory, but
      30             :   we also save a lot of CPU (approx 10% on STM32F427) as the compiler
      31             :   is able to resolve the address of these variables at compile time,
      32             :   which means significantly faster code
      33             :  */
      34             : class NavEKF_core_common {
      35             : public:
      36             : #if MATH_CHECK_INDEXES
      37             :     typedef VectorN<ftype,28> Vector28;
      38             :     typedef VectorN<VectorN<ftype,24>,24> Matrix24;
      39             : #else
      40             :     typedef ftype Vector28[28];
      41             :     typedef ftype Matrix24[24][24];
      42             : #endif
      43             : 
      44             : protected:
      45             :     static Matrix24 KHP;                  // intermediate result used for covariance updates
      46             :     static Vector28 Kfusion;              // intermediate fusion vector
      47             : 
      48             :     // fill all the common scratch variables with NaN on SITL
      49             :     void fill_scratch_variables(void);
      50             : 
      51             :     // zero part of an array for index range [n1,n2]
      52  1607942829 :     static void zero_range(ftype *v, uint8_t n1, uint8_t n2) {
      53  1607942829 :         memset(&v[n1], 0, sizeof(ftype)*(1+(n2-n1)));
      54  1607942829 :     }
      55             : };
      56             : 
      57             : #if HAL_WITH_EKF_DOUBLE && !defined(__clang__)
      58             : // stack frames are larger with double EKF
      59             : #if MATH_CHECK_INDEXES
      60             : #pragma GCC diagnostic error "-Wframe-larger-than=4000"
      61             : #else
      62             : #pragma GCC diagnostic error "-Wframe-larger-than=2600"
      63             : #endif
      64             : #endif
      65             :