Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
vi_datalink.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 The Paparazzi Team
3  *
4  * This is the implementation of the "external interface" to the autopilot.
5  * using datalink messages.
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2 of the License, or
10  * (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, write to the Free Software
19  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
20  */
21 
22 #ifndef VEHICLE_INTERFACE_DATALINK_H
23 #define VEHICLE_INTERFACE_DATALINK_H
24 
25 #include "std.h"
26 #include "modules/vehicle_interface/vi.h"
27 #include "math/pprz_algebra_int.h"
28 
29 #ifndef VI_MAX_H_SPEED
30 #define VI_MAX_H_SPEED 4.
31 #endif
32 
33 #ifndef VI_MAX_V_SPEED
34 #define VI_MAX_V_SPEED 2.
35 #endif
36 
37 #ifndef VI_MAX_HEADING_RATE
38 #define VI_MAX_HEADING_RATE RadOfDeg(60.)
39 #endif
40 
41 extern void vi_update_wp(uint8_t wp_id);
42 
43 #ifdef VI_PHI_THETA_MAX
44 #define VI_LIMIT_ATTITUDE(_att) { \
45  BoundAbs(_att.phi, VI_PHI_THETA_MAX); \
46  BoundAbs(_att.theta, VI_PHI_THETA_MAX); \
47  }
48 #else
49 #define VI_LIMIT_ATTITUDE(_x) {}
50 #endif
51 
52 #define VI_PARSE_DATALINK(_dl_buffer) { \
53  vi.last_msg = 0; \
54  vi.input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer); \
55  vi.input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer); \
56  switch (vi.input.h_mode) { \
57  case GUIDANCE_H_MODE_KILL: \
58  case GUIDANCE_H_MODE_RATE : \
59  break; \
60  case GUIDANCE_H_MODE_ATTITUDE : \
61  { \
62  vi.input.h_sp.attitude.phi = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \
63  vi.input.h_sp.attitude.theta = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \
64  vi.input.h_sp.attitude.psi = DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer); \
65  ANGLE_REF_NORMALIZE(vi.input.h_sp.attitude.psi); \
66  VI_LIMIT_ATTITUDE(vi.input.h_sp.attitude); \
67  } \
68  break; \
69  case GUIDANCE_H_MODE_HOVER : \
70  { \
71  vi.input.h_sp.pos.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \
72  vi.input.h_sp.pos.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \
73  } \
74  break; \
75  case GUIDANCE_H_MODE_NAV : \
76  { \
77  vi.input.h_sp.speed.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \
78  vi.input.h_sp.speed.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \
79  vi.input.h_sp.speed.z = DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer); \
80  } \
81  break; \
82  default: \
83  break; \
84  } \
85  switch (vi.input.v_mode) { \
86  case GUIDANCE_V_MODE_KILL: \
87  case GUIDANCE_V_MODE_RC_DIRECT: \
88  case GUIDANCE_V_MODE_RC_CLIMB: \
89  break; \
90  case GUIDANCE_V_MODE_CLIMB : \
91  vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
92  break; \
93  case GUIDANCE_V_MODE_HOVER : \
94  vi.input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
95  break; \
96  case GUIDANCE_V_MODE_NAV : \
97  vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
98  break; \
99  default: \
100  break; \
101  } \
102  }
103 
104 #define VI_NAV_STICK_PARSE_DL(_dl_buffer) { \
105  vi.last_msg = 0; \
106  vi.input.h_mode = GUIDANCE_H_MODE_NAV; \
107  vi.input.v_mode = GUIDANCE_V_MODE_NAV; \
108  vi.input.h_sp.speed.x = DL_BOOZ_NAV_STICK_vx_sp(_dl_buffer); \
109  vi.input.h_sp.speed.y = DL_BOOZ_NAV_STICK_vy_sp(_dl_buffer); \
110  vi.input.h_sp.speed.z = DL_BOOZ_NAV_STICK_r_sp(_dl_buffer); \
111  vi.input.v_sp.climb = DL_BOOZ_NAV_STICK_vz_sp(_dl_buffer); \
112  }
113 
114 #define NavUpdateWPFromVI(_wp) { if (vi.enabled) { vi_update_wp(uint8_t _wp); } }
115 
116 #endif /* VI_DATALINK_H */
vi_update_wp
void vi_update_wp(uint8_t wp_id)
Definition: vi_datalink.c:42
pprz_algebra_int.h
Paparazzi fixed point algebra.
std.h
uint8_t
unsigned char uint8_t
Definition: types.h:14
vi.h