Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 
34 {
39  fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
40  fms.input.v_mode = GUIDANCE_V_MODE_HOVER;
41 }
42 
44 {
45 
46  switch (fms_test_signal.mode) {
47 
48  case STEP_ROLL: {
50  EULERS_ASSIGN(fms.input.h_sp.attitude, fms_test_signal.amplitude, 0, 0);
51  } else {
52  EULERS_ASSIGN(fms.input.h_sp.attitude, -fms_test_signal.amplitude, 0, 0);
53  }
54  }
55  break;
56  case STEP_YAW: {
58  EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, fms_test_signal.amplitude);
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_impl.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 
87 void booz_fms_impl_set_enabled(bool enabled)
88 {
89  if (enabled) {
91  }
92 }
struct BoozFmsTestSignal fms_test_signal
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:54
Integrated Navigation System interface.
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:273
void booz_fms_impl_periodic(void)
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:39
#define FMS_TEST_SIGNAL_DEFAULT_PERIOD
uint8_t guidance_v_mode
Definition: guidance_v.c:99
#define FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE
void booz_fms_impl_set_enabled(bool enabled)
#define FMS_TEST_SIGNAL_DEFAULT_MODE
void booz_fms_impl_init(void)
Paparazzi fixed point algebra.
enum fms_ts_mode mode