Paparazzi UAS  v6.0_unstable-41-g5cfda00-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
gvf_parametric_low_level_control.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
27 #include "autopilot.h"
29 #include "gvf_parametric.h"
30 
31 #if defined(FIXEDWING_FIRMWARE)
33 #include "firmwares/fixedwing/guidance/guidance_v_n.h" // gvf_parametric is only compatible with the new pprz controller!
34 #endif
35 
36 void gvf_parametric_low_level_control_2D(float heading_rate)
37 {
38 #if defined(FIXEDWING_FIRMWARE)
40  // Lateral XY coordinates
42 
43  struct FloatEulers *att = stateGetNedToBodyEulers_f();
45 
47  -gvf_parametric_control.k_roll * atanf(heading_rate * ground_speed / GVF_PARAMETRIC_GRAVITY / cosf(att->theta));
48  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle
49  }
50 #else
51 #error gvf_parametric does not support your firmware yet
52 #endif
53 }
54 
55 void gvf_parametric_low_level_control_3D(float heading_rate, float climbing_rate)
56 {
57 #if defined(FIXEDWING_FIRMWARE)
59  // Vertical Z coordinate
62 
63  v_ctl_climb_setpoint = gvf_parametric_control.k_climb * climbing_rate; // Setting point for vertical speed
64 
65  // Lateral XY coordinates
67 
68  struct FloatEulers *att = stateGetNedToBodyEulers_f();
70 
72  -gvf_parametric_control.k_roll * atanf(heading_rate * ground_speed / GVF_PARAMETRIC_GRAVITY / cosf(att->theta));
73  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle
74  }
75 #else
76 #error gvf_parametric does not support your firmware yet
77 #endif
78 }
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
uint8_t lateral_mode
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static float ground_speed
Definition: follow_me.c:76
void gvf_parametric_low_level_control_2D(float heading_rate)
float v_ctl_climb_setpoint
Definition: energy_ctrl.c:98
"New" vertical control for fixed wing vehicles.
#define GVF_PARAMETRIC_GRAVITY
euler angles
Fixed wing horizontal control.
float theta
in radians
#define AP_MODE_AUTO2
#define V_CTL_SPEED_THROTTLE
Definition: energy_ctrl.h:36
uint8_t v_ctl_speed_mode
Definition: energy_ctrl.c:135
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
#define V_CTL_MODE_AUTO_CLIMB
float h_ctl_roll_max_setpoint
Core autopilot interface common to all firmwares.
gvf_parametric_con gvf_parametric_control
#define LATERAL_MODE_ROLL
float h_ctl_roll_setpoint
Guiding vector field algorithm for 2D and 3D parametric trajectories.
void gvf_parametric_low_level_control_3D(float heading_rate, float climbing_rate)
Firmware dependent file for the guiding vector field algorithm for 2D and 3D parametric trajectories...
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184