Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 }
v_ctl_mode
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
V_CTL_SPEED_THROTTLE
#define V_CTL_SPEED_THROTTLE
Definition: energy_ctrl.h:36
AP_MODE_AUTO2
#define AP_MODE_AUTO2
Definition: autopilot_static.h:38
ground_speed
static float ground_speed
Definition: follow_me.c:76
LATERAL_MODE_ROLL
#define LATERAL_MODE_ROLL
Definition: autopilot_firmware.h:38
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
h_ctl_roll_max_setpoint
float h_ctl_roll_max_setpoint
Definition: stabilization_adaptive.c:85
stabilization_attitude.h
h_ctl_roll_setpoint
float h_ctl_roll_setpoint
Definition: stabilization_adaptive.c:157
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
gvf_parametric_control
gvf_parametric_con gvf_parametric_control
Definition: gvf_parametric.cpp:44
lateral_mode
uint8_t lateral_mode
Definition: autopilot_firmware.c:38
gvf_parametric.h
gvf_parametric_low_level_control_3D
void gvf_parametric_low_level_control_3D(float heading_rate, float climbing_rate)
Definition: gvf_parametric_low_level_control.c:55
gvf_parametric_con::k_roll
float k_roll
Definition: gvf_parametric.h:82
v_ctl_climb_setpoint
float v_ctl_climb_setpoint
Definition: energy_ctrl.c:98
autopilot.h
gvf_parametric_low_level_control.h
guidance_v_n.h
gvf_parametric_con::k_climb
float k_climb
Definition: gvf_parametric.h:83
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
GVF_PARAMETRIC_GRAVITY
#define GVF_PARAMETRIC_GRAVITY
Definition: gvf_parametric.h:30
gvf_parametric_low_level_control_2D
void gvf_parametric_low_level_control_2D(float heading_rate)
Definition: gvf_parametric_low_level_control.c:36
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:216
V_CTL_MODE_AUTO_CLIMB
#define V_CTL_MODE_AUTO_CLIMB
Definition: guidance_common.h:38
v_ctl_speed_mode
uint8_t v_ctl_speed_mode
Definition: energy_ctrl.c:135