Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 }
ins.h
GUIDANCE_H_MODE_ATTITUDE
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:58
STEP_YAW
@ STEP_YAW
Definition: vi_test_signal.h:31
EULERS_ASSIGN
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:274
STEP_VERT
@ STEP_VERT
Definition: vi_test_signal.h:32
BoozFmsTestSignal::counter
uint32_t counter
Definition: vi_test_signal.h:39
pprz_algebra_int.h
Paparazzi fixed point algebra.
guidance_v_mode
uint8_t guidance_v_mode
Definition: guidance_v.c:103
BoozFmsTestSignal::period
uint32_t period
Definition: vi_test_signal.h:37
BoozFmsTestSignal::mode
enum fms_ts_mode mode
Definition: vi_test_signal.h:36
booz_fms_impl_init
void booz_fms_impl_init(void)
Definition: vi_test_signal.c:33
FMS_TEST_SIGNAL_DEFAULT_MODE
#define FMS_TEST_SIGNAL_DEFAULT_MODE
Definition: vi_test_signal.c:27
STEP_ROLL
@ STEP_ROLL
Definition: vi_test_signal.h:29
fms_test_signal
struct BoozFmsTestSignal fms_test_signal
Definition: vi_test_signal.c:31
GUIDANCE_V_MODE_HOVER
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:39
booz_fms_impl_set_enabled
void booz_fms_impl_set_enabled(bool enabled)
Definition: vi_test_signal.c:87
booz_fms_impl_periodic
void booz_fms_impl_periodic(void)
Definition: vi_test_signal.c:43
BoozFmsTestSignal::amplitude
uint32_t amplitude
Definition: vi_test_signal.h:38
FMS_TEST_SIGNAL_DEFAULT_PERIOD
#define FMS_TEST_SIGNAL_DEFAULT_PERIOD
Definition: vi_test_signal.c:28
FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE
#define FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE
Definition: vi_test_signal.c:29
STEP_PITCH
@ STEP_PITCH
Definition: vi_test_signal.h:30
BoozFmsTestSignal
Definition: vi_test_signal.h:35