Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
vi_datalink.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2010 The Paparazzi Team
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 
25 
26 void vi_impl_init(void) {
27 }
28 
29 void vi_impl_periodic(void) {
30 }
31 
32 void vi_impl_set_enabled(bool_t enabled __attribute__ ((unused))) {
33 }
34 
35 #define ViMaxHSpeed ((int16_t)SPEED_BFP_OF_REAL(VI_MAX_H_SPEED))
36 #define ViMaxVSpeed ((int16_t)SPEED_BFP_OF_REAL(VI_MAX_V_SPEED))
37 #define ViMaxHeadingRate ((int16_t)RATE_BFP_OF_REAL(VI_MAX_HEADING_RATE))
38 
40 
41 void vi_update_wp(uint8_t wp_id) {
42  struct Int16Vect3 wp_speed;
43  wp_speed.x = ViMaxHSpeed * vi.input.h_sp.speed.x / 128;
44  wp_speed.y = ViMaxHSpeed * vi.input.h_sp.speed.y / 128;
45  wp_speed.z = ViMaxVSpeed * vi.input.v_sp.climb / 128;
46  VECT3_BOUND_BOX(wp_speed, wp_speed, wp_speed_max);
47  int16_t heading_rate = vi.input.h_sp.speed.z;
48  BoundAbs(heading_rate, ViMaxHeadingRate);
49  navigation_update_wp_from_speed(wp_id , wp_speed, heading_rate);
50 
51 }
struct VehicleInterface vi
Definition: vi.c:32
struct Vi_command input
Definition: vi.h:84
#define VECT3_BOUND_BOX(_v, _v_min, _v_max)
Definition: pprz_algebra.h:204
union Vi_command::@14 h_sp
bool_t enabled
Definition: vi.h:80
signed short int16_t
Definition: types.h:17
unsigned char uint8_t
Definition: types.h:14
union Vi_command::@15 v_sp
int32_t climb
Definition: vi.h:72
struct Int32Vect3 speed
Definition: vi.h:67
__attribute__((always_inline))
Definition: i2c_arch.c:35