Paparazzi UAS  v7.0_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 
73 extern struct OpticalFlowHover of_hover;
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_ */
float cov_setpoint
for adaptive gain control, setpoint of the covariance (oscillations)
uint8_t cov_array_filledXY
float d_err
difference of error for the D-gain
float P
P-gain for control.
int32_t PID_divergence_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
Determine and set the thrust for constant divergence control.
struct OpticalFlowHover of_hover
float divergence
Divergence estimate.
struct GainsPID PID
The struct with the PID gains.
float flowX
Flow estimate in X direction.
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...
float previous_err
Previous tracking error.
float I
I-gain for control.
float input[COV_WINDOW_SIZE]
float PID_flow_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
Determine and set the desired angle for constant flow control.
float reduction_factor
Reduce the gain by this factor when oscillating.
float sum_err
integration of the error for I-gain
float past_OF[COV_WINDOW_SIZE]
uint32_t ind_histXY
float setpoint
setpoint for constant divergence/flow
float D
D-gain for control.
uint32_t ind_histZ
uint8_t cov_array_filledZ
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...
float flowY
Flow estimate in Y direction.
float nominal_value
The nominal value of thrust, phi or theta depending on Z, Y, X.
float OF[COV_WINDOW_SIZE]
float err
Current tracking error.
float ramp
The ramp pused is increased with per dt.
#define COV_WINDOW_SIZE
struct OFhistory historyX
struct OFhistory historyY
bool cov_method
method to calculate the covariance: between thrust and div / angle and flow (0) or div and div past /...
Paparazzi floating point algebra.
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98