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
vi_test_signal.c
Go to the documentation of this file.
1 /*
2  * $Id: booz_fms_test_signal.c 4675 2010-03-13 22:59:46Z poine $
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 "booz_fms.h"
25 
26 #include "subsystems/ins.h"
27 #include "math/pprz_algebra_int.h"
28 
29 #define FMS_TEST_SIGNAL_DEFAULT_MODE STEP_YAW
30 #define FMS_TEST_SIGNAL_DEFAULT_PERIOD 40
31 #define FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE (((int32_t)ANGLE_BFP_OF_REAL(RadOfDeg(5)))<<8)
32 
34 
35 void booz_fms_impl_init(void) {
40  fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
41  fms.input.v_mode = GUIDANCE_V_MODE_HOVER;
42 }
43 
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  }
52  else {
53  EULERS_ASSIGN(fms.input.h_sp.attitude, -fms_test_signal.amplitude, 0, 0);
54  }
55  }
56  break;
57  case STEP_YAW: {
59  EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, fms_test_signal.amplitude);
60  }
61  else {
62  EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, -fms_test_signal.amplitude);
63  }
64  }
65  break;
66  case STEP_PITCH:
67  case STEP_VERT:
68  break;
69 #if 0
70  case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: {
72  booz_fms_test_signal_start_z = ins_ltp_pos.z;
73  else {
74  booz_fms_input.v_sp.height = (booz_fms_test_signal_counter < booz_fms_test_signal_period) ?
75  booz_fms_test_signal_start_z :
76  booz_fms_test_signal_start_z - 256;
77  //BOOZ_INT_OF_FLOAT(-0.5, IPOS_FRAC)
78  }
79  }
80  break;
81 #endif
82  }
86 }
87 
88 void booz_fms_impl_set_enabled(bool_t enabled) {
89  if (enabled)
91 }
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:242
struct BoozFmsTestSignal fms_test_signal
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:37
Device independent INS code.
struct NedCoor_i ins_ltp_pos
Definition: ins.c:77
void booz_fms_impl_periodic(void)
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:52
void booz_fms_impl_set_enabled(bool_t enabled)
#define FMS_TEST_SIGNAL_DEFAULT_PERIOD
uint8_t guidance_v_mode
Definition: guidance_v.c:57
#define FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE
#define FMS_TEST_SIGNAL_DEFAULT_MODE
enum fms_ts_mode mode
void booz_fms_impl_init(void)
Paparazzi fixed point algebra.