Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
optical_flow_functions.c
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 #include "optical_flow_functions.h"
22 #include "paparazzi.h"
23 #include "math/pprz_stat.h"
24 #include "std.h"
25 
26 // The maximum bank angle the algorithm can steer
27 #ifndef OFH_MAXBANK
28 #define OFH_MAXBANK 10.f
29 #endif
30 
31 // The low pass filter constant for updating the vision measurements in the algorithm
32 #ifndef OF_LP_CONST
33 #define OF_LP_CONST 0.5 // Average between 0.4 Bebop value and 0.6 ARDrone value
34 #endif
35 
36 // In case the autocovariance is used, select half the window size as the delay
37 #ifndef OF_COV_DELAY_STEPS
38 #define OF_COV_DELAY_STEPS COV_WINDOW_SIZE/2
39 #endif
40 
45 
46 
52 float set_cov_div(bool cov_method, struct OFhistory *history, struct DesiredInputs *inputs)
53 {
54  float cov_div = 0;
55  // histories and cov detection:
56 
57  history->OF[ind_histZ] = of_hover.divergence;
58 
59  float normalized_thrust = (float)(100.0 * inputs->thrust / MAX_PPRZ);
60  history->input[ind_histZ] = normalized_thrust;
61 
62  int ind_past = ind_histZ - OF_COV_DELAY_STEPS;
63  while (ind_past < 0) { ind_past += COV_WINDOW_SIZE; }
64  history->past_OF[ind_histZ] = history->OF[ind_past];
65 
66  // determine the covariance for hover detection:
67  // only take covariance into account if there are enough samples in the histories:
68  if (cov_method == 0 && cov_array_filledZ > 0) {
69  // TODO: step in hover set point causes an incorrectly perceived covariance
70  cov_div = covariance_f(history->input, history->OF, COV_WINDOW_SIZE);
71  } else if (cov_method == 1 && cov_array_filledZ > 1) {
72  // todo: delay steps should be invariant to the run frequency
73  cov_div = covariance_f(history->past_OF, history->OF, COV_WINDOW_SIZE);
74  }
75 
76  if (cov_array_filledZ < 2 && ind_histZ + 1 == COV_WINDOW_SIZE) {
78  }
80 
81  return cov_div;
82 }
83 
84 
89 void set_cov_flow(bool cov_method, struct OFhistory *historyX, struct OFhistory *historyY, struct DesiredInputs *inputs,
90  struct FloatVect3 *covs)
91 {
92  // histories and cov detection:
95 
96  int ind_past = ind_histXY - OF_COV_DELAY_STEPS;
97  while (ind_past < 0) { ind_past += COV_WINDOW_SIZE; }
98  historyX->past_OF[ind_histXY] = historyX->OF[ind_past];
99  historyY->past_OF[ind_histXY] = historyY->OF[ind_past];
100  float normalized_phi = (float)(100.0 * inputs->phi / OFH_MAXBANK);
101  float normalized_theta = (float)(100.0 * inputs->theta / OFH_MAXBANK);
102  historyX->input[ind_histXY] = normalized_phi;
103  historyY->input[ind_histXY] = normalized_theta;
104 
105  // determine the covariance for hover detection:
106  // only take covariance into account if there are enough samples in the histories:
107  if (cov_method == 0 && cov_array_filledXY > 0) {
108  // // TODO: step in hover set point causes an incorrectly perceived covariance
111  } else if (cov_method == 1 && cov_array_filledXY > 1) {
112  if (cov_array_filledXY > 1) {
113  // todo: delay steps should be invariant to the run frequency
116  }
117  }
118 
119  if (cov_array_filledXY < 2 && ind_histXY + 1 == COV_WINDOW_SIZE) {
121  }
123 }
124 
125 
132 float PID_flow_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
133 {
134  // update the controller errors:
135  float lp_factor = dt / OF_LP_CONST;
136  Bound(lp_factor, 0.f, 1.f);
137 
138  // maintain the controller errors:
139  of_hover_ctrl->PID.sum_err += of_hover_ctrl->PID.err;
140  of_hover_ctrl->PID.d_err += (((of_hover_ctrl->PID.err - of_hover_ctrl->PID.previous_err) / dt) -
141  of_hover_ctrl->PID.d_err) * lp_factor;
142  of_hover_ctrl->PID.previous_err = of_hover_ctrl->PID.err;
143 
144  // compute the desired angle
145  float des_angle = of_hover_ctrl->PID.P * of_hover_ctrl->PID.err + of_hover_ctrl->PID.I *
146  of_hover_ctrl->PID.sum_err + of_hover_ctrl->PID.D * of_hover_ctrl->PID.d_err;
147 
148  // Bound angle:
149  Bound(des_angle, -OFH_MAXBANK, OFH_MAXBANK);
150 
151  return des_angle;
152 }
153 
161 {
162  // update the controller errors:
163  float lp_factor = dt / OF_LP_CONST;
164  Bound(lp_factor, 0.f, 1.f);
165 
166  // maintain the controller errors:
167  of_hover_ctrl->PID.sum_err += of_hover_ctrl->PID.err;
168  of_hover_ctrl->PID.d_err += (((of_hover_ctrl->PID.err - of_hover_ctrl->PID.previous_err) / dt) -
169  of_hover_ctrl->PID.d_err) * lp_factor;
170  of_hover_ctrl->PID.previous_err = of_hover_ctrl->PID.err;
171 
172  // PID control:
173  int32_t thrust = (of_hover_ctrl->nominal_value
174  + of_hover_ctrl->PID.P * of_hover_ctrl->PID.err
175  + of_hover_ctrl->PID.I * of_hover_ctrl->PID.sum_err
176  + of_hover_ctrl->PID.D * of_hover_ctrl->PID.d_err) * MAX_PPRZ;
177 
178  // bound thrust:
179  Bound(thrust, 0.25 * of_hover_ctrl->nominal_value * MAX_PPRZ, MAX_PPRZ);
180 
181  return thrust;
182 }
GainsPID::err
float err
Current tracking error.
Definition: optical_flow_functions.h:31
ind_histZ
uint32_t ind_histZ
Definition: optical_flow_functions.c:43
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
GainsPID::D
float D
D-gain for control.
Definition: optical_flow_functions.h:30
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
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
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
cov_array_filledZ
uint8_t cov_array_filledZ
Definition: optical_flow_functions.c:44
uint32_t
unsigned long uint32_t
Definition: types.h:18
OFH_MAXBANK
#define OFH_MAXBANK
Definition: optical_flow_functions.c:28
DesiredInputs
Definition: optical_flow_functions.h:62
paparazzi.h
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
std.h
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
OFhistory::OF
float OF[COV_WINDOW_SIZE]
Definition: optical_flow_functions.h:39
pprz_stat.h
Statistics functions.
uint8_t
unsigned char uint8_t
Definition: types.h:14
cov_array_filledXY
uint8_t cov_array_filledXY
Definition: optical_flow_functions.c:42
covariance_f
float covariance_f(float *arr1, float *arr2, uint32_t n_elements)
Compute the covariance of two arrays V(X) = E[(X-E[X])(Y-E[Y])] = E[XY] - E[X]E[Y] where E[X] is the ...
Definition: pprz_stat.c:152
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
COV_WINDOW_SIZE
#define COV_WINDOW_SIZE
Definition: optical_flow_hover.c:63
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
of_hover
struct OpticalFlowHover of_hover
Definition: optical_flow_functions.h:73
OpticalFlowHover::divergence
float divergence
Divergence estimate.
Definition: optical_flow_functions.h:57
historyX
struct OFhistory historyX
Definition: optical_flow_hover.c:190
OF_LP_CONST
#define OF_LP_CONST
Definition: optical_flow_functions.c:33
DesiredInputs::phi
float phi
Definition: optical_flow_functions.h:63
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
OFhistory
Definition: optical_flow_functions.h:37
int32_t
signed long int32_t
Definition: types.h:19
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
optical_flow_functions.h
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
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
cov_div
float cov_div
Definition: optical_flow_landing.c:136
OF_COV_DELAY_STEPS
#define OF_COV_DELAY_STEPS
Definition: optical_flow_functions.c:38
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
normalized_thrust
float normalized_thrust
Definition: optical_flow_landing.c:135
ind_histXY
uint32_t ind_histXY
Definition: optical_flow_functions.c:41
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