Paparazzi UAS  v5.15_devel-46-gd23ed71
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/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
68  AbiBindMsgAGL(VERTICAL_CTRL_MODULE_AGL_ID, &agl_ev, vertical_ctrl_agl_cb);
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 }
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
struct VerticalCtrlDemo v_ctrl
void vertical_ctrl_module_init(void)
Main include for ABI (AirBorneInterface).
void guidance_v_module_enter(void)
Entering the vertical module (user switched to module)
static void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
Callback function of the ground altitude.
void guidance_v_module_run(bool in_flight)
void vertical_ctrl_module_run(bool in_flight)
void guidance_v_module_init(void)
unsigned long uint32_t
Definition: types.h:18
#define VERTICAL_CTRL_MODULE_IGAIN
signed long int32_t
Definition: types.h:19
#define VERTICAL_CTRL_MODULE_PGAIN
unsigned char uint8_t
Definition: types.h:14
General stabilization interface for rotorcrafts.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
#define VERTICAL_CTRL_MODULE_AGL_ID
#define MAX_PPRZ
Definition: paparazzi.h:8
static abi_event agl_ev
The altitude ABI event.