PX4 Firmware
PX4 Autopilot Software http://px4.io
pid.h
Go to the documentation of this file.
1
/****************************************************************************
2
*
3
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
4
* Author: Laurens Mackay <mackayl@student.ethz.ch>
5
* Tobias Naegeli <naegelit@student.ethz.ch>
6
* Martin Rutschmann <rutmarti@student.ethz.ch>
7
* Anton Babushkin <anton.babushkin@me.com>
8
* Julian Oes <joes@student.ethz.ch>
9
*
10
* Redistribution and use in source and binary forms, with or without
11
* modification, are permitted provided that the following conditions
12
* are met:
13
*
14
* 1. Redistributions of source code must retain the above copyright
15
* notice, this list of conditions and the following disclaimer.
16
* 2. Redistributions in binary form must reproduce the above copyright
17
* notice, this list of conditions and the following disclaimer in
18
* the documentation and/or other materials provided with the
19
* distribution.
20
* 3. Neither the name PX4 nor the names of its contributors may be
21
* used to endorse or promote products derived from this software
22
* without specific prior written permission.
23
*
24
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
31
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
32
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35
* POSSIBILITY OF SUCH DAMAGE.
36
*
37
****************************************************************************/
38
39
/**
40
* @file pid.h
41
*
42
* Definition of generic PID controller.
43
*
44
* @author Laurens Mackay <mackayl@student.ethz.ch>
45
* @author Tobias Naegeli <naegelit@student.ethz.ch>
46
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
47
* @author Anton Babushkin <anton.babushkin@me.com>
48
* @author Julian Oes <joes@student.ethz.ch>
49
*/
50
51
#ifndef PID_H_
52
#define PID_H_
53
54
#include <stdint.h>
55
56
__BEGIN_DECLS
57
58
typedef
enum
PID_MODE
{
59
/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
60
PID_MODE_DERIVATIV_NONE
= 0,
61
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
62
* val_dot in pid_calculate() will be ignored */
63
PID_MODE_DERIVATIV_CALC
,
64
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
65
* setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
66
PID_MODE_DERIVATIV_CALC_NO_SP
,
67
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
68
PID_MODE_DERIVATIV_SET
69
}
pid_mode_t
;
70
71
typedef
struct
{
72
pid_mode_t
mode
;
73
float
dt_min
;
74
float
kp
;
75
float
ki
;
76
float
kd
;
77
float
integral
;
78
float
integral_limit
;
79
float
output_limit
;
80
float
error_previous
;
81
float
last_output
;
82
}
PID_t
;
83
84
__EXPORT
void
pid_init
(
PID_t
*pid,
pid_mode_t
mode
,
float
dt_min);
85
__EXPORT
int
pid_set_parameters
(
PID_t
*pid,
float
kp,
float
ki,
float
kd,
float
integral_limit,
float
output_limit);
86
__EXPORT
float
pid_calculate
(
PID_t
*pid,
float
sp,
float
val,
float
val_dot,
float
dt
);
87
__EXPORT
void
pid_reset_integral
(
PID_t
*pid);
88
89
__END_DECLS
90
91
#endif
/* PID_H_ */
PID_t::last_output
float last_output
Definition:
pid.h:81
PID_t::integral_limit
float integral_limit
Definition:
pid.h:78
__END_DECLS
#define __END_DECLS
Definition:
visibility.h:59
PID_t::error_previous
float error_previous
Definition:
pid.h:80
pid_init
__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
Definition:
pid.cpp:57
__EXPORT
Definition:
I2C.hpp:51
PID_t::kp
float kp
Definition:
pid.h:74
PID_MODE_DERIVATIV_CALC
Definition:
pid.h:63
PID_MODE_DERIVATIV_CALC_NO_SP
Definition:
pid.h:66
PID_t::mode
pid_mode_t mode
Definition:
pid.h:72
pid_mode_t
__BEGIN_DECLS enum PID_MODE pid_mode_t
pid_reset_integral
__EXPORT void pid_reset_integral(PID_t *pid)
Definition:
pid.cpp:182
PID_MODE
PID_MODE
Definition:
pid.h:58
__BEGIN_DECLS
#define __BEGIN_DECLS
Definition:
visibility.h:58
PID_t::ki
float ki
Definition:
pid.h:75
PID_t::dt_min
float dt_min
Definition:
pid.h:73
PID_t::integral
float integral
Definition:
pid.h:77
PID_t
Definition:
pid.h:71
PID_t::kd
float kd
Definition:
pid.h:76
PID_MODE_DERIVATIV_SET
Definition:
pid.h:68
dt
float dt
Definition:
px4io.c:73
pid_calculate
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
Definition:
pid.cpp:113
PID_t::output_limit
float output_limit
Definition:
pid.h:79
pid_set_parameters
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
Definition:
pid.cpp:71
PID_MODE_DERIVATIV_NONE
Definition:
pid.h:60
mode
mode
Definition:
vtol_type.h:76
src
lib
pid
pid.h
Generated by
1.8.13