flight/libraries/math/mathmisc.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilot Math Utilities
   4  * @{
   5  * @addtogroup Reuseable math functions
   6  * @{
   7  *
   8  * @file       mathmisc.c
   9  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  10  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  11  * @brief      Reuseable math functions
  12  *
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include <mathmisc.h>
  33
  34 void pseudo_windowed_variance_init(pw_variance_t *variance, int32_t window_size)
  35 {
  36     variance->new_sma  = 0.0f;
  37     variance->new_smsa = 0.0f;
  38     variance->p1 = 1.0f / (float)window_size;
  39     variance->p2 = 1.0f - variance->p1;
  40 }
  41
  42 float pseudo_windowed_variance_get(pw_variance_t *variance)
  43 {
  44     return variance->new_smsa - variance->new_sma * variance->new_sma;
  45 }