Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
optical_flow_functions.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018
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 
21 #ifndef OPTICAL_FLOW_FUNCTIONS_H_
22 #define OPTICAL_FLOW_FUNCTIONS_H_
23 
24 #include "std.h"
26 
27 struct GainsPID {
28  float P;
29  float I;
30  float D;
31  float err;
32  float previous_err;
33  float sum_err;
34  float d_err;
35 };
36 
37 struct OFhistory {
41 };
42 
44  struct GainsPID PID;
45 
46  float nominal_value;
47 
48  float ramp;
49 
51 
52  float setpoint;
53  float cov_setpoint;
54 };
55 
57  float divergence;
58  float flowX;
59  float flowY;
60 };
61 
62 struct DesiredInputs {
63  float phi;
64  float theta;
66 };
67 
68 extern uint32_t ind_histXY;
70 extern uint32_t ind_histZ;
72 
74 
75 extern float set_cov_div(bool cov_method, struct OFhistory *history, struct DesiredInputs *inputs);
76 extern void set_cov_flow(bool cov_method, struct OFhistory *historyX, struct OFhistory *historyY,
77  struct DesiredInputs *inputs, struct FloatVect3 *covs);
78 extern float PID_flow_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl);
79 extern int32_t PID_divergence_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl);
80 #endif /* OPTICAL_FLOW_FUNCTIONS_H_ */
GainsPID::err
float err
Current tracking error.
Definition: optical_flow_functions.h:31
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
GainsPID::D
float D
D-gain for control.
Definition: optical_flow_functions.h:30
PID_divergence_control
int32_t PID_divergence_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
Determine and set the thrust for constant divergence control.
Definition: optical_flow_functions.c:160
OFhistory::past_OF
float past_OF[COV_WINDOW_SIZE]
Definition: optical_flow_functions.h:40
DesiredInputs::theta
float theta
Definition: optical_flow_functions.h:64
set_cov_flow
void set_cov_flow(bool cov_method, struct OFhistory *historyX, struct OFhistory *historyY, struct DesiredInputs *inputs, struct FloatVect3 *covs)
Set the covariance of the flow and past flow / desired angle This funciton should only be called once...
Definition: optical_flow_functions.c:89
cov_array_filledZ
uint8_t cov_array_filledZ
Definition: optical_flow_functions.c:44
DesiredInputs::thrust
int32_t thrust
Definition: optical_flow_functions.h:65
OpticalFlowHoverControl::PID
struct GainsPID PID
The struct with the PID gains.
Definition: optical_flow_functions.h:44
DesiredInputs
Definition: optical_flow_functions.h:62
pprz_algebra_float.h
Paparazzi floating point algebra.
ind_histXY
uint32_t ind_histXY
Definition: optical_flow_functions.c:41
set_cov_div
float set_cov_div(bool cov_method, struct OFhistory *history, struct DesiredInputs *inputs)
Set the covariance of the divergence and the thrust / past divergence This funciton should only be ca...
Definition: optical_flow_functions.c:52
cov_array_filledXY
uint8_t cov_array_filledXY
Definition: optical_flow_functions.c:42
OFhistory::input
float input[COV_WINDOW_SIZE]
Definition: optical_flow_functions.h:38
GainsPID::I
float I
I-gain for control.
Definition: optical_flow_functions.h:29
GainsPID::previous_err
float previous_err
Previous tracking error.
Definition: optical_flow_functions.h:32
historyY
struct OFhistory historyY
Definition: optical_flow_hover.c:191
FloatVect3
Definition: pprz_algebra_float.h:54
OpticalFlowHoverControl::ramp
float ramp
The ramp pused is increased with per dt.
Definition: optical_flow_functions.h:48
std.h
ind_histZ
uint32_t ind_histZ
Definition: optical_flow_functions.c:43
cov_method
bool cov_method
method to calculate the covariance: between thrust and div / angle and flow (0) or div and div past /...
Definition: optical_flow_hover.c:213
OpticalFlowHover::flowX
float flowX
Flow estimate in X direction.
Definition: optical_flow_functions.h:58
OpticalFlowHoverControl::setpoint
float setpoint
setpoint for constant divergence/flow
Definition: optical_flow_functions.h:52
OFhistory::OF
float OF[COV_WINDOW_SIZE]
Definition: optical_flow_functions.h:39
OpticalFlowHoverControl::reduction_factor
float reduction_factor
Reduce the gain by this factor when oscillating.
Definition: optical_flow_functions.h:50
COV_WINDOW_SIZE
#define COV_WINDOW_SIZE
Definition: optical_flow_hover.c:63
of_hover
struct OpticalFlowHover of_hover
Definition: optical_flow_functions.h:73
OpticalFlowHover::divergence
float divergence
Divergence estimate.
Definition: optical_flow_functions.h:57
GainsPID
Definition: optical_flow_functions.h:27
historyX
struct OFhistory historyX
Definition: optical_flow_hover.c:190
OpticalFlowHover
Definition: optical_flow_functions.h:56
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
DesiredInputs::phi
float phi
Definition: optical_flow_functions.h:63
PID_flow_control
float PID_flow_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
Determine and set the desired angle for constant flow control.
Definition: optical_flow_functions.c:132
OFhistory
Definition: optical_flow_functions.h:37
GainsPID::sum_err
float sum_err
integration of the error for I-gain
Definition: optical_flow_functions.h:33
OpticalFlowHoverControl
Definition: optical_flow_functions.h:43
OpticalFlowHoverControl::nominal_value
float nominal_value
The nominal value of thrust, phi or theta depending on Z, Y, X.
Definition: optical_flow_functions.h:46
OpticalFlowHover::flowY
float flowY
Flow estimate in Y direction.
Definition: optical_flow_functions.h:59
OpticalFlowHoverControl::cov_setpoint
float cov_setpoint
for adaptive gain control, setpoint of the covariance (oscillations)
Definition: optical_flow_functions.h:53
GainsPID::P
float P
P-gain for control.
Definition: optical_flow_functions.h:28
GainsPID::d_err
float d_err
difference of error for the D-gain
Definition: optical_flow_functions.h:34