Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2010 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
28 #include "subsystems/ahrs.h"
29 
30 #if USE_AHRS_ALIGNER
32 #endif
33 
34 
35 #ifndef PRIMARY_AHRS
36 #error "PRIMARY_AHRS not set!"
37 #else
38 PRINT_CONFIG_VAR(PRIMARY_AHRS)
39 #endif
40 
41 #ifdef SECONDARY_AHRS
42 PRINT_CONFIG_VAR(SECONDARY_AHRS)
43 #endif
44 
45 #define __RegisterAhrs(_x) _x ## _register()
46 #define _RegisterAhrs(_x) __RegisterAhrs(_x)
47 #define RegisterAhrs(_x) _RegisterAhrs(_x)
48 
50 #ifndef AHRS_NB_IMPL
51 #define AHRS_NB_IMPL 2
52 #endif
53 
55 struct AhrsImpl {
57 };
58 
61 
63 {
64  int i;
65  for (i=0; i < AHRS_NB_IMPL; i++) {
66  if (ahrs_impls[i].enable == NULL) {
68  break;
69  }
70  }
71 }
72 
73 void ahrs_init(void)
74 {
75  int i;
76  for (i=0; i < AHRS_NB_IMPL; i++) {
77  ahrs_impls[i].enable = NULL;
78  }
79 
81 #ifdef SECONDARY_AHRS
82  RegisterAhrs(SECONDARY_AHRS);
83 #endif
84 
85  // enable primary AHRS by default
86  ahrs_switch(0);
87 
88 #if USE_AHRS_ALIGNER
90 #endif
91 }
92 
94 {
95  if (idx >= AHRS_NB_IMPL) { return -1; }
96  if (ahrs_impls[idx].enable == NULL) { return -1; }
97  /* first disable other AHRS output */
98  int i;
99  for (i=0; i < AHRS_NB_IMPL; i++) {
100  if (ahrs_impls[i].enable != NULL) {
101  ahrs_impls[i].enable(FALSE);
102  }
103  }
104  /* enable requested AHRS */
106  ahrs_output_idx = idx;
107  return ahrs_output_idx;
108 }
Interface to align the AHRS via low-passed measurements at startup.
static uint32_t idx
Dispatcher to register actual AHRS implementations.
AhrsEnableOutput enable
Definition: ahrs.c:56
#define PRIMARY_AHRS
Definition: ahrs_chimu.h:40
bool(* AhrsEnableOutput)(bool)
Definition: ahrs.h:55
#define RegisterAhrs(_x)
Definition: ahrs.c:47
void ahrs_init(void)
AHRS initialization.
Definition: ahrs.c:73
struct AhrsImpl ahrs_impls[AHRS_NB_IMPL]
Definition: ahrs.c:59
#define FALSE
Definition: std.h:5
#define AHRS_NB_IMPL
maximum number of AHRS implementations that can register
Definition: ahrs.c:51
#define TRUE
Definition: std.h:4
int ahrs_switch(uint8_t idx)
Switch to the output of another AHRS impl.
Definition: ahrs.c:93
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
Definition: ahrs.c:62
references a registered AHRS implementation
Definition: ahrs.c:55
uint8_t ahrs_output_idx
Definition: ahrs.c:60
unsigned char uint8_t
Definition: types.h:14
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:81