Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ahrs_aligner.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
24 #include "ahrs_aligner.h"
25 
26 #include <stdlib.h> /* for abs() */
27 #include "subsystems/imu.h"
28 #include "led.h"
29 
31 
32 #define SAMPLES_NB PERIODIC_FREQUENCY
33 
34 static struct Int32Rates gyro_sum;
35 static struct Int32Vect3 accel_sum;
36 static struct Int32Vect3 mag_sum;
39 
40 void ahrs_aligner_init(void) {
41 
46  samples_idx = 0;
47  ahrs_aligner.noise = 0;
49 }
50 
51 #ifndef LOW_NOISE_THRESHOLD
52 #define LOW_NOISE_THRESHOLD 90000
53 #endif
54 #ifndef LOW_NOISE_TIME
55 #define LOW_NOISE_TIME 5
56 #endif
57 
58 void ahrs_aligner_run(void) {
59 
63 
65  samples_idx++;
66 
67 #ifdef AHRS_ALIGNER_LED
68  RunOnceEvery(50, {LED_TOGGLE(AHRS_ALIGNER_LED);});
69 #endif
70 
71  if (samples_idx >= SAMPLES_NB) {
72  int32_t avg_ref_sensor = accel_sum.z;
73  if ( avg_ref_sensor >= 0)
74  avg_ref_sensor += SAMPLES_NB / 2;
75  else
76  avg_ref_sensor -= SAMPLES_NB / 2;
77  avg_ref_sensor /= SAMPLES_NB;
78 
79  ahrs_aligner.noise = 0;
80  int i;
81  for (i=0; i<SAMPLES_NB; i++) {
82  int32_t diff = ref_sensor_samples[i] - avg_ref_sensor;
83  ahrs_aligner.noise += abs(diff);
84  }
85 
88  VECT3_SDIV(ahrs_aligner.lp_mag, mag_sum, SAMPLES_NB);
89 
93  samples_idx = 0;
94 
97  else
98  if ( ahrs_aligner.low_noise_cnt > 0)
100 
103 #ifdef AHRS_ALIGNER_LED
104  LED_ON(AHRS_ALIGNER_LED);
105 #endif
106  }
107  }
108 
109 }
#define SAMPLES_NB
Definition: ahrs_aligner.c:32
#define LOW_NOISE_THRESHOLD
Definition: ahrs_aligner.c:52
static struct Int32Vect3 mag_sum
Definition: ahrs_aligner.c:36
int32_t low_noise_cnt
Definition: ahrs_aligner.h:39
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:36
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define LED_ON(i)
Definition: led_hw.h:28
#define INT_RATES_ZERO(_e)
uint8_t status
Definition: ahrs_aligner.h:40
int32_t noise
Definition: ahrs_aligner.h:38
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:37
#define AHRS_ALIGNER_RUNNING
Definition: ahrs_aligner.h:31
#define RATES_SDIV(_ro, _ri, _s)
Definition: pprz_algebra.h:352
static struct Int32Rates gyro_sum
Definition: ahrs_aligner.c:34
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:30
unsigned long uint32_t
Definition: types.h:18
#define AHRS_ALIGNER_LOCKED
Definition: ahrs_aligner.h:32
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:310
#define INT_VECT3_ZERO(_v)
Inertial Measurement Unit interface.
angular rates
#define LED_TOGGLE(i)
Definition: led_hw.h:30
#define LOW_NOISE_TIME
Definition: ahrs_aligner.c:55
signed long int32_t
Definition: types.h:19
struct Int32Vect3 mag
magnetometer measurements
Definition: imu.h:42
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:120
struct Int32Rates gyro
gyroscope measurements
Definition: imu.h:40
arch independent LED (Light Emitting Diodes) API
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
static int32_t ref_sensor_samples[SAMPLES_NB]
Definition: ahrs_aligner.c:37
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:169
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:40
void ahrs_aligner_run(void)
Definition: ahrs_aligner.c:58
static uint32_t samples_idx
Definition: ahrs_aligner.c:38
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:35
static struct Int32Vect3 accel_sum
Definition: ahrs_aligner.c:35