Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
vertical_ctrl_module_demo.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015
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 
28 
29 #include "generated/airframe.h"
30 #include "paparazzi.h"
31 #include "modules/core/abi.h"
33 
34 /* Default sonar/agl to use */
35 #ifndef VERTICAL_CTRL_MODULE_AGL_ID
36 #define VERTICAL_CTRL_MODULE_AGL_ID ABI_BROADCAST
37 #endif
38 PRINT_CONFIG_VAR(VERTICAL_CTRL_MODULE_AGL_ID)
39 
40 #ifndef VERTICAL_CTRL_MODULE_PGAIN
41 #define VERTICAL_CTRL_MODULE_PGAIN 1.0
42 #endif
43 
44 #ifndef VERTICAL_CTRL_MODULE_IGAIN
45 #define VERTICAL_CTRL_MODULE_IGAIN 0.01
46 #endif
47 
48 static abi_event agl_ev;
49 
51 static void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
52 
54 
55 
56 void vertical_ctrl_module_init(void);
57 void vertical_ctrl_module_run(bool in_flight);
58 
60 {
61  v_ctrl.agl = 0.0f;
62  v_ctrl.setpoint = 1.0f;
65  v_ctrl.sum_err = 0.0f;
66 
67  // Subscribe to the altitude above ground level ABI messages
69 }
70 
71 
72 void vertical_ctrl_module_run(bool in_flight)
73 {
74  if (!in_flight) {
75  // Reset integrators
76  v_ctrl.sum_err = 0;
77  stabilization_cmd[COMMAND_THRUST] = 0;
78  } else {
79  int32_t nominal_throttle = 0.5 * MAX_PPRZ;
80  float err = v_ctrl.setpoint - v_ctrl.agl;
81  int32_t thrust = nominal_throttle + v_ctrl.pgain * err + v_ctrl.igain * v_ctrl.sum_err;
82  Bound(thrust, 0, MAX_PPRZ);
83  stabilization_cmd[COMMAND_THRUST] = thrust;
84  v_ctrl.sum_err += err;
85  }
86 }
87 
88 static void vertical_ctrl_agl_cb(__attribute__((unused)) uint8_t sender_id, __attribute__((unused)) uint32_t stamp, float distance)
89 {
90  v_ctrl.agl = distance;
91 }
92 
93 
95 // Call our controller
97 {
99 }
100 
102 {
103  // reset integrator
104  v_ctrl.sum_err = 0.0f;
105 }
106 
107 void guidance_v_module_run(bool in_flight)
108 {
109  vertical_ctrl_module_run(in_flight);
110 }
guidance_v_module_run
void guidance_v_module_run(bool in_flight)
Definition: vertical_ctrl_module_demo.c:107
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
agl_ev
static abi_event agl_ev
The altitude ABI event.
Definition: vertical_ctrl_module_demo.c:48
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
guidance_v_module_enter
void guidance_v_module_enter(void)
Entering the vertical module (user switched to module)
Definition: vertical_ctrl_module_demo.c:101
VerticalCtrlDemo::igain
float igain
Definition: vertical_ctrl_module_demo.h:37
abi.h
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
vertical_ctrl_agl_cb
static void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
Callback function of the ground altitude.
Definition: vertical_ctrl_module_demo.c:88
VerticalCtrlDemo::agl
float agl
Definition: vertical_ctrl_module_demo.h:34
paparazzi.h
VERTICAL_CTRL_MODULE_PGAIN
#define VERTICAL_CTRL_MODULE_PGAIN
Definition: vertical_ctrl_module_demo.c:41
vertical_ctrl_module_run
void vertical_ctrl_module_run(bool in_flight)
Definition: vertical_ctrl_module_demo.c:72
v_ctrl
struct VerticalCtrlDemo v_ctrl
Definition: vertical_ctrl_module_demo.c:53
VerticalCtrlDemo::sum_err
float sum_err
Definition: vertical_ctrl_module_demo.h:38
guidance_v_module_init
void guidance_v_module_init(void)
Definition: vertical_ctrl_module_demo.c:96
vertical_ctrl_module_init
void vertical_ctrl_module_init(void)
Definition: vertical_ctrl_module_demo.c:59
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
VerticalCtrlDemo
Definition: vertical_ctrl_module_demo.h:33
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
VerticalCtrlDemo::setpoint
float setpoint
Definition: vertical_ctrl_module_demo.h:35
stabilization.h
VERTICAL_CTRL_MODULE_IGAIN
#define VERTICAL_CTRL_MODULE_IGAIN
Definition: vertical_ctrl_module_demo.c:45
vertical_ctrl_module_demo.h
example vertical
VerticalCtrlDemo::pgain
float pgain
Definition: vertical_ctrl_module_demo.h:36
VERTICAL_CTRL_MODULE_AGL_ID
#define VERTICAL_CTRL_MODULE_AGL_ID
Definition: vertical_ctrl_module_demo.c:36