Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
vi_test_signal.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
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 
22 #include "booz_fms.h"
23 
24 #include "subsystems/ins.h"
25 #include "math/pprz_algebra_int.h"
26 
27 #define FMS_TEST_SIGNAL_DEFAULT_MODE STEP_YAW
28 #define FMS_TEST_SIGNAL_DEFAULT_PERIOD 40
29 #define FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE (((int32_t)ANGLE_BFP_OF_REAL(RadOfDeg(5)))<<8)
30 
32 
33 void booz_fms_impl_init(void) {
38  fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
39  fms.input.v_mode = GUIDANCE_V_MODE_HOVER;
40 }
41 
43 
44  switch (fms_test_signal.mode) {
45 
46  case STEP_ROLL: {
48  EULERS_ASSIGN(fms.input.h_sp.attitude, fms_test_signal.amplitude, 0, 0);
49  }
50  else {
51  EULERS_ASSIGN(fms.input.h_sp.attitude, -fms_test_signal.amplitude, 0, 0);
52  }
53  }
54  break;
55  case STEP_YAW: {
57  EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, fms_test_signal.amplitude);
58  }
59  else {
60  EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, -fms_test_signal.amplitude);
61  }
62  }
63  break;
64  case STEP_PITCH:
65  case STEP_VERT:
66  break;
67 #if 0
68  case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: {
70  booz_fms_test_signal_start_z = ins_ltp_pos.z;
71  else {
72  booz_fms_input.v_sp.height = (booz_fms_test_signal_counter < booz_fms_test_signal_period) ?
73  booz_fms_test_signal_start_z :
74  booz_fms_test_signal_start_z - 256;
75  //BOOZ_INT_OF_FLOAT(-0.5, IPOS_FRAC)
76  }
77  }
78  break;
79 #endif
80  }
84 }
85 
86 void booz_fms_impl_set_enabled(bool_t enabled) {
87  if (enabled)
89 }
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:240
struct BoozFmsTestSignal fms_test_signal
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:46
Integrated Navigation System interface.
int32_t z
Down.
void booz_fms_impl_periodic(void)
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:41
void booz_fms_impl_set_enabled(bool_t enabled)
#define FMS_TEST_SIGNAL_DEFAULT_PERIOD
uint8_t guidance_v_mode
Definition: guidance_v.c:54
#define FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE
struct NedCoor_i ins_ltp_pos
Definition: ins_ardrone2.c:50
#define FMS_TEST_SIGNAL_DEFAULT_MODE
enum fms_ts_mode mode
void booz_fms_impl_init(void)
Paparazzi fixed point algebra.