Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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:
93  historyX->OF[ind_histXY] = of_hover.flowX;
94  historyY->OF[ind_histXY] = of_hover.flowY;
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
109  covs->x = covariance_f(historyX->input, historyX->OF, COV_WINDOW_SIZE);
110  covs->y = covariance_f(historyY->input, historyY->OF, COV_WINDOW_SIZE);
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
114  covs->x = covariance_f(historyX->past_OF, historyX->OF, COV_WINDOW_SIZE);
115  covs->y = covariance_f(historyY->past_OF, historyY->OF, COV_WINDOW_SIZE);
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 }
float normalized_thrust
uint32_t ind_histZ
float past_OF[COV_WINDOW_SIZE]
float previous_err
Previous tracking error.
struct GainsPID PID
The struct with the PID gains.
float I
I-gain for control.
#define OF_COV_DELAY_STEPS
struct OpticalFlowHover of_hover
#define OFH_MAXBANK
float sum_err
integration of the error for I-gain
bool cov_method
method to calculate the covariance: between thrust and div / angle and flow (0) or div and div past /...
unsigned long uint32_t
Definition: types.h:18
float flowY
Flow estimate in Y direction.
float input[COV_WINDOW_SIZE]
float err
Current tracking error.
#define COV_WINDOW_SIZE
signed long int32_t
Definition: types.h:19
#define OF_LP_CONST
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
float flowX
Flow estimate in X direction.
uint32_t ind_histXY
float nominal_value
The nominal value of thrust, phi or theta depending on Z, Y, X.
float P
P-gain for control.
float OF[COV_WINDOW_SIZE]
unsigned char uint8_t
Definition: types.h:14
uint8_t cov_array_filledZ
int32_t PID_divergence_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
Determine and set the thrust for constant divergence control.
Statistics functions.
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...
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 d_err
difference of error for the D-gain
float cov_div
struct OFhistory historyX
#define MAX_PPRZ
Definition: paparazzi.h:8
float D
D-gain for control.
float divergence
Divergence estimate.
uint8_t cov_array_filledXY
struct OFhistory historyY
float PID_flow_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
Determine and set the desired angle for constant flow control.