rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-19 04:52:12 +01:00
Code Issues Releases Activity

LP-327 - implement some kind of pseudo variance to be used to determine if copter is steady

Browse Source
This commit is contained in:
Alessio Morale 2016-06-25 20:38:58 +02:00
parent 5ca24b6d25
commit 9774045699
2 changed files with 45 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
flight/libraries/math/mathmisc.c
View File

@ -28,5 +28,17 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <mathmisc.h>
// space deliberately left empty, any non inline misc math functions can go here
void pseudo_windowed_variance_init(pw_variance_t *variance, int32_t window_size)
{
variance->new_sma = 0.0f;
variance->new_smsa = 0.0f;
variance->p1 = 1.0f / (float)window_size;
variance->p2 = 1.0f - variance->p1;
}
float pseudo_windowed_variance_get(pw_variance_t *variance)
{
return variance->new_smsa - variance->new_sma * variance->new_sma;
}

32
flight/libraries/math/mathmisc.h
View File

@ -34,6 +34,38 @@
#include <math.h>
#include <stdint.h>
typedef struct {
float p1;
float p2;
float new_sma;
float new_smsa;
} pw_variance_t;
/***
* initialize pseudo windowed
* @param variance the instance to be initialized
* @param window_size size of the sample window
*/
void pseudo_windowed_variance_init(pw_variance_t *variance, int32_t window_size);
/***
* Push a new sample
* @param variance the working instance
* @param sample the new sample
*/
static inline void pseudo_windowed_variance_push_sample(pw_variance_t *variance, float sample)
{
variance->new_sma = variance->new_sma * variance->p2 + sample * variance->p1;
variance->new_smsa = variance->new_smsa * variance->p2 + sample * sample * variance->p1;
}
/***
* Get the current variance value
* @param variance the working instance
* @return
*/
float pseudo_windowed_variance_get(pw_variance_t *variance);
// returns min(boundary1,boundary2) if val<min(boundary1,boundary2)
// returns max(boundary1,boundary2) if val>max(boundary1,boundary2)
// returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2)