Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
optical_flow_landing.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Guido de Croon.
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 
44 #include "optical_flow_landing.h"
45 
46 #include "generated/airframe.h"
47 #include "paparazzi.h"
48 #include "modules/core/abi.h"
51 
52 // used for automated landing:
53 #include "autopilot.h"
56 
57 // for measuring time
58 #include "mcu_periph/sys_time.h"
59 
60 #include "math/pprz_stat.h"
61 
62 /* Default sonar/agl to use */
63 #ifndef OFL_AGL_ID
64 #define OFL_AGL_ID ABI_BROADCAST
65 #endif
66 PRINT_CONFIG_VAR(OFL_AGL_ID)
67 
68 /* Use optical flow estimates */
69 #ifndef OFL_OPTICAL_FLOW_ID
70 #define OFL_OPTICAL_FLOW_ID ABI_BROADCAST
71 #endif
72 PRINT_CONFIG_VAR(OFL_OPTICAL_FLOW_ID)
73 
74 // Other default values:
75 #ifndef OFL_PGAIN
76 #define OFL_PGAIN 0.40
77 #endif
78 
79 #ifndef OFL_IGAIN
80 #define OFL_IGAIN 0.01
81 #endif
82 
83 #ifndef OFL_DGAIN
84 #define OFL_DGAIN 0.0
85 #endif
86 
87 #ifndef OFL_VISION_METHOD
88 #define OFL_VISION_METHOD 1
89 #endif
90 
91 #ifndef OFL_CONTROL_METHOD
92 #define OFL_CONTROL_METHOD 0
93 #endif
94 
95 #ifndef OFL_COV_METHOD
96 #define OFL_COV_METHOD 0
97 #endif
98 
99 // number of time steps used for calculating the covariance (oscillations)
100 #ifndef OFL_COV_WINDOW_SIZE
101 #define OFL_COV_WINDOW_SIZE 30
102 #endif
103 
104 #ifndef OFL_COV_LANDING_LIMIT
105 #define OFL_COV_LANDING_LIMIT 2.2
106 #endif
107 
108 #ifndef OFL_COV_SETPOINT
109 #define OFL_COV_SETPOINT -0.0075
110 #endif
111 
112 #ifndef OFL_LP_CONST
113 #define OFL_LP_CONST 0.02
114 #endif
115 
116 #ifndef OFL_P_LAND_THRESHOLD
117 #define OFL_P_LAND_THRESHOLD 0.15
118 #endif
119 
120 #ifndef OFL_ELC_OSCILLATE
121 #define OFL_ELC_OSCILLATE true
122 #endif
123 
124 // Constants
125 // minimum value of the P-gain for divergence control
126 // adaptive control / exponential gain control will not be able to go lower
127 #define MINIMUM_GAIN 0.1
128 
129 // for exponential gain landing, gain increase per second during the first (hover) phase:
130 #define INCREASE_GAIN_PER_SECOND 0.02
131 
132 // variables retained between module calls
136 float cov_div;
137 float pstate, pused;
138 float istate;
139 float dstate;
141 bool landing;
145 
146 // for the exponentially decreasing gain strategy:
152 
155 
156 // struct containing most relevant parameters
158 
159 // sending the divergence message to the ground station:
160 static void send_divergence(struct transport_tx *trans, struct link_device *dev)
161 {
162  pprz_msg_send_DIVERGENCE(trans, dev, AC_ID,
165 }
166 
169 void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
170 // Callback function of the optical flow estimate:
171 void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x,
172  int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
173 
174 // common functions for different landing strategies:
175 static void set_cov_div(int32_t thrust);
176 static int32_t PID_divergence_control(float divergence_setpoint, float P, float I, float D, float dt);
177 static void update_errors(float error, float dt);
178 static uint32_t final_landing_procedure(void);
179 
180 // resetting all variables to be called for instance when starting up / re-entering module
181 static void reset_all_vars(void);
182 
188 
189 void vertical_ctrl_module_init(void);
190 void vertical_ctrl_module_run(bool in_flight);
191 
196 {
197  // filling the of_landing_ctrl struct with default values:
198  of_landing_ctrl.agl = 0.0f;
199  of_landing_ctrl.agl_lp = 0.0f;
200  of_landing_ctrl.vel = 0.0f;
201  of_landing_ctrl.divergence_setpoint = 0.0f; // For exponential gain landing, pick a negative value
205  Bound(of_landing_ctrl.lp_const, 0.001f, 1.f);
211  of_landing_ctrl.sum_err = 0.0f;
212  of_landing_ctrl.d_err = 0.0f;
213  of_landing_ctrl.nominal_thrust = (float)guidance_v_nominal_throttle / MAX_PPRZ; // copy this value from guidance
223  0.80f; // for exponential gain landing, after detecting oscillations, the gain is multiplied with this factor
225  0.99f; // low pass filtering cov div so that the drone is really oscillating when triggering the descent
227  // if the gain reaches this value during an exponential landing, the drone makes the final landing.
230  reset_all_vars();
231 
232  // Subscribe to the altitude above ground level ABI messages
233  AbiBindMsgAGL(OFL_AGL_ID, &agl_ev, vertical_ctrl_agl_cb);
234  // Subscribe to the optical flow estimator:
235  // register telemetry:
238 }
239 
243 static void reset_all_vars(void)
244 {
246 
248 
249  cov_div = 0.;
251  previous_cov_err = 0.;
252  divergence_vision = 0.;
255 
258 
259  ind_hist = 0;
260  cov_array_filled = 0;
261  uint32_t i;
262  for (i = 0; i < OFL_COV_WINDOW_SIZE; i++) {
263  thrust_history[i] = 0;
264  divergence_history[i] = 0;
265  }
266 
267  landing = false;
268 
269  elc_phase = 0;
270  elc_time_start = 0;
271  count_covdiv = 0;
272  lp_cov_div = 0.0f;
273 
275  pused = pstate;
278 
282  of_landing_ctrl.d_err = 0.;
283 }
284 
288 void vertical_ctrl_module_run(bool in_flight)
289 {
290  float div_factor; // factor that maps divergence in pixels as received from vision to 1 / frame
291 
292  float dt = vision_time - prev_vision_time;
293 
294  // check if new measurement received
295  if (dt <= 1e-5f) {
296  return;
297  }
298 
299  Bound(of_landing_ctrl.lp_const, 0.001f, 1.f);
300  float lp_factor = dt / of_landing_ctrl.lp_const;
301  Bound(lp_factor, 0.f, 1.f);
302 
303  /***********
304  * VISION
305  ***********/
306  if (of_landing_ctrl.VISION_METHOD == 0) {
307  // SIMULATED DIVERGENCE:
308 
309  // USE OPTITRACK HEIGHT
311 
312  if (fabsf(of_landing_ctrl.agl - of_landing_ctrl.agl_lp) > 1.0f) {
313  // ignore outliers:
315  }
316  // calculate the new low-pass height and the velocity
318 
319  // only calculate velocity and divergence if dt is large enough:
321 
322  // calculate the fake divergence:
323  if (of_landing_ctrl.agl_lp > 1e-5f) {
325  // TODO: this time scaling should be done in optical flow module
327  if (fabsf(divergence_vision_dt) > 1e-5f) {
329  }
330  }
331  } else {
332  // USE REAL VISION OUTPUTS:
333  // TODO: this div_factor depends on the subpixel-factor (automatically adapt?)
334  // TODO: this factor is camera specific and should be implemented in the optical
335  // flow calculator module not here. Additionally, the time scaling should also
336  // be done in the calculator module
337  div_factor = -1.28f; // magic number comprising field of view etc.
338  float new_divergence = (divergence_vision * div_factor) / dt;
339 
340  // deal with (unlikely) fast changes in divergence:
341  static const float max_div_dt = 0.20f;
342  if (fabsf(new_divergence - of_landing_ctrl.divergence) > max_div_dt) {
343  if (new_divergence < of_landing_ctrl.divergence) { new_divergence = of_landing_ctrl.divergence - max_div_dt; }
344  else { new_divergence = of_landing_ctrl.divergence + max_div_dt; }
345  }
346 
347  // low-pass filter the divergence:
348  of_landing_ctrl.divergence += (new_divergence - of_landing_ctrl.divergence) * lp_factor;
350  }
351 
352  /***********
353  * CONTROL
354  ***********/
355  // landing indicates whether the drone is already performing a final landing procedure (flare):
356  if (!landing) {
357  if (of_landing_ctrl.CONTROL_METHOD == 0) {
358  // FIXED GAIN CONTROL, cov_limit for landing:
359 
360  // use the divergence for control:
362  of_landing_ctrl.dgain, dt);
363 
364  // trigger the landing if the cov div is too high:
365  if (fabsf(cov_div) > of_landing_ctrl.cov_limit) {
367  }
368  } else if (of_landing_ctrl.CONTROL_METHOD == 1) {
369  // ADAPTIVE GAIN CONTROL:
370  // TODO: i-gain and d-gain are currently not adapted
371 
372  // adapt the gains according to the error in covariance:
373  float error_cov = of_landing_ctrl.cov_set_point - cov_div;
374  // limit the error_cov, which could else become very large:
375  if (error_cov > fabsf(of_landing_ctrl.cov_set_point)) { error_cov = fabsf(of_landing_ctrl.cov_set_point); }
376  pstate -= (of_landing_ctrl.igain_adaptive * pstate) * error_cov;
377  if (pstate < MINIMUM_GAIN) { pstate = MINIMUM_GAIN; }
378  pused = pstate - (of_landing_ctrl.pgain_adaptive * pstate) * error_cov;
379  // make sure pused does not become too small, nor grows too fast:
380  if (pused < MINIMUM_GAIN) { pused = MINIMUM_GAIN; }
381  if (of_landing_ctrl.COV_METHOD == 1 && error_cov > 0.001) {
382  pused = 0.5 * pused;
383  }
384 
385  // use the divergence for control:
387  of_landing_ctrl.dgain, dt);
388 
389  // when to make the final landing:
392  }
393 
394  } else if (of_landing_ctrl.CONTROL_METHOD == 2) {
395  // EXPONENTIAL GAIN CONTROL:
396  static const float phase_0_set_point = 0.0f;
397  if (elc_phase == 0) {
398  // increase the gain till you start oscillating:
399 
400  // if not yet oscillating, increase the gains:
403  float gain_factor = pstate / pused;
404  istate *= gain_factor;
405  dstate *= gain_factor;
406  pused = pstate;
407  }
408 
409  // use the divergence for control:
410  thrust_set = PID_divergence_control(phase_0_set_point, pused, istate, dstate, dt);
411 
412  // low pass filter cov div and remove outliers:
413  if (fabsf(lp_cov_div - cov_div) < of_landing_ctrl.cov_limit) {
415  }
416  // if oscillating, maintain a counter to see if it endures:
418  count_covdiv++;
419  } else {
420  count_covdiv = 0;
422  }
423  // if the drone has been oscillating long enough, start landing:
426  // next phase:
427  elc_phase = 1;
429 
430  // we don't want to oscillate, so reduce the gain:
434  count_covdiv = 0;
435  of_landing_ctrl.sum_err = 0.0f;
436  }
437  } else if (elc_phase == 1) {
438  // control divergence to 0 with the reduced gain:
440  pused = pstate;
443 
444  float t_interval = get_sys_time_float() - elc_time_start;
445  // this should not happen, but just to be sure to prevent too high gain values:
446  if (t_interval < 0) { t_interval = 0.0f; }
447 
448  // use the divergence for control:
449  thrust_set = PID_divergence_control(phase_0_set_point, pused, istate, dstate, dt);
450 
451  // if we have been trying to hover stably again for 2 seconds and we move in the same way as the desired divergence, switch to landing:
452  if (t_interval >= 2.0f && of_landing_ctrl.divergence * of_landing_ctrl.divergence_setpoint >= 0.0f) {
453  // next phase:
454  elc_phase = 2;
456  count_covdiv = 0;
457  }
458  } else if (elc_phase == 2) {
459  // land while exponentially decreasing the gain:
460  float t_interval = get_sys_time_float() - elc_time_start;
461 
462  // this should not happen, but just to be sure to prevent too high gain values:
463  if (t_interval < 0) { t_interval = 0.0f; }
464 
465  // determine the P-gain, exponentially decaying:
466  float gain_scaling = expf(of_landing_ctrl.divergence_setpoint * t_interval);
467  pstate = elc_p_gain_start * gain_scaling;
468  istate = elc_i_gain_start * gain_scaling;
469  dstate = elc_d_gain_start * gain_scaling;
470  pused = pstate;
471 
472  // 2 [1/s] ramp to setpoint
473  /*if (fabsf(of_landing_ctrl.divergence_setpoint - divergence_setpoint) > 2.*dt){
474  divergence_setpoint += 2*dt * of_landing_ctrl.divergence_setpoint / fabsf(of_landing_ctrl.divergence_setpoint);
475  } else {
476  divergence_setpoint = of_landing_ctrl.divergence_setpoint;
477  }*/
478 
479  // use the divergence for control:
481 
482  // when to make the final landing:
484  elc_phase = 3;
485  }
486  } else {
488  }
489  }
490 
491  if (in_flight) {
493  stabilization_cmd[COMMAND_THRUST] = thrust_set;
494  }
495 
496  }
497 }
498 
503 {
504  // land with 85% nominal thrust:
505  uint32_t nominal_throttle = of_landing_ctrl.nominal_thrust * MAX_PPRZ;
506  uint32_t thrust = 0.85 * nominal_throttle;
507  Bound(thrust, 0.6 * nominal_throttle, 0.9 * MAX_PPRZ);
508  landing = true;
509 
510  return thrust;
511 }
512 
518 void set_cov_div(int32_t thrust)
519 {
520  // histories and cov detection:
522 
523  normalized_thrust = (float)(thrust / (MAX_PPRZ / 100));
525 
526  int ind_past = ind_hist - of_landing_ctrl.delay_steps;
527  while (ind_past < 0) { ind_past += of_landing_ctrl.window_size; }
529 
530  // determine the covariance for landing detection:
531  // only take covariance into account if there are enough samples in the histories:
532  if (of_landing_ctrl.COV_METHOD == 0 && cov_array_filled > 0) {
533  // TODO: step in landing set point causes an incorrectly perceived covariance
535  } else if (of_landing_ctrl.COV_METHOD == 1 && cov_array_filled > 1) {
536  // todo: delay steps should be invariant to the run frequency
538  }
539 
542  }
544 }
545 
555 int32_t PID_divergence_control(float setpoint, float P, float I, float D, float dt)
556 {
557  // determine the error:
558  float err = setpoint - of_landing_ctrl.divergence;
559 
560  // update the controller errors:
561  update_errors(err, dt);
562 
563  // PID control:
565  + P * err
568 
569  // bound thrust:
570  Bound(thrust, 0.25 * of_landing_ctrl.nominal_thrust * MAX_PPRZ, MAX_PPRZ);
571 
572  // update covariance
573  set_cov_div(thrust);
574 
575  return thrust;
576 }
577 
583 void update_errors(float err, float dt)
584 {
585  float lp_factor = dt / of_landing_ctrl.lp_const;
586  Bound(lp_factor, 0.f, 1.f);
587 
588  // maintain the controller errors:
589  of_landing_ctrl.sum_err += err;
590  of_landing_ctrl.d_err += (((err - of_landing_ctrl.previous_err) / dt) - of_landing_ctrl.d_err) * lp_factor;
592 }
593 
594 // Reading from "sensors":
595 void vertical_ctrl_agl_cb(uint8_t sender_id UNUSED, __attribute__((unused)) uint32_t stamp, float distance)
596 {
597  of_landing_ctrl.agl = distance;
598 }
599 
601  int32_t flow_x UNUSED, int32_t flow_y UNUSED,
602  int32_t flow_der_x UNUSED, int32_t flow_der_y UNUSED, float quality UNUSED, float size_divergence)
603 {
604  divergence_vision = size_divergence;
605  vision_time = ((float)stamp) / 1e6;
606 }
607 
609 // Call our controller
611 {
613 }
614 
619 {
620  reset_all_vars();
621 
622  // adaptive estimation - assume hover condition when entering the module
623  of_landing_ctrl.nominal_thrust = (float) stabilization_cmd[COMMAND_THRUST] / MAX_PPRZ;
625 }
626 
627 void guidance_v_module_run(bool in_flight)
628 {
629  vertical_ctrl_module_run(in_flight);
630 }
optical_flow_ev
static abi_event optical_flow_ev
Definition: optical_flow_landing.c:154
vertical_ctrl_module_init
void vertical_ctrl_module_init(void)
Initialize the optical flow landing module.
Definition: optical_flow_landing.c:195
OFL_COV_METHOD
#define OFL_COV_METHOD
Definition: optical_flow_landing.c:96
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
OFL_OPTICAL_FLOW_ID
#define OFL_OPTICAL_FLOW_ID
Definition: optical_flow_landing.c:70
thrust_set
int32_t thrust_set
Definition: optical_flow_landing.c:143
common_flight_plan.h
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
guidance_v_adapt.h
OpticalFlowLanding::window_size
uint32_t window_size
number of time steps in "window" used for getting the covariance
Definition: optical_flow_landing.h:72
set_cov_div
static void set_cov_div(int32_t thrust)
Set the covariance of the divergence and the thrust / past divergence This funciton should only be ca...
Definition: optical_flow_landing.c:518
send_divergence
static void send_divergence(struct transport_tx *trans, struct link_device *dev)
Definition: optical_flow_landing.c:160
PID_divergence_control
static int32_t PID_divergence_control(float divergence_setpoint, float P, float I, float D, float dt)
Determine and set the thrust for constant divergence control.
Definition: optical_flow_landing.c:555
elc_i_gain_start
float elc_i_gain_start
Definition: optical_flow_landing.c:149
OpticalFlowLanding::VISION_METHOD
uint32_t VISION_METHOD
whether to use vision (1) or Optitrack / sonar (0)
Definition: optical_flow_landing.h:63
INCREASE_GAIN_PER_SECOND
#define INCREASE_GAIN_PER_SECOND
Definition: optical_flow_landing.c:130
OFL_COV_WINDOW_SIZE
#define OFL_COV_WINDOW_SIZE
Definition: optical_flow_landing.c:101
OpticalFlowLanding::t_transition
float t_transition
how many seconds the drone has to be oscillating in order to transition to the hover phase with reduc...
Definition: optical_flow_landing.h:75
abi.h
landing
bool landing
Definition: optical_flow_landing.c:141
guidance_v_module_init
void guidance_v_module_init(void)
Definition: optical_flow_landing.c:610
elc_time_start
uint32_t elc_time_start
Definition: optical_flow_landing.c:148
get_sys_time_float
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
divergence_vision_dt
float divergence_vision_dt
Definition: optical_flow_landing.c:134
OpticalFlowLanding::lp_cov_div_factor
float lp_cov_div_factor
low-pass factor for the covariance of divergence in order to trigger the second landing phase in the ...
Definition: optical_flow_landing.h:74
update_errors
static void update_errors(float error, float dt)
Updates the integral and differential errors for PID control and sets the previous error.
Definition: optical_flow_landing.c:583
divergence_history
float divergence_history[OFL_COV_WINDOW_SIZE]
Definition: optical_flow_landing.c:184
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
OpticalFlowLanding::nominal_thrust
float nominal_thrust
nominal thrust around which the PID-control operates
Definition: optical_flow_landing.h:62
OpticalFlowLanding::reduction_factor_elc
float reduction_factor_elc
reduction factor - after oscillation, how much to reduce the gain...
Definition: optical_flow_landing.h:73
UNUSED
uint8_t last_wp UNUSED
Definition: navigation.c:101
vertical_ctrl_agl_cb
void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
Function definitions Callback function of the ground altitude.
Definition: optical_flow_landing.c:595
OFL_ELC_OSCILLATE
#define OFL_ELC_OSCILLATE
Definition: optical_flow_landing.c:121
OpticalFlowLanding::agl_lp
float agl_lp
low-pass version of agl
Definition: optical_flow_landing.h:51
OFL_DGAIN
#define OFL_DGAIN
Definition: optical_flow_landing.c:84
pstate
float pstate
Definition: optical_flow_landing.c:137
OpticalFlowLanding::COV_METHOD
uint32_t COV_METHOD
method to calculate the covariance: between thrust and div (0) or div and div past (1)
Definition: optical_flow_landing.h:70
OFL_COV_SETPOINT
#define OFL_COV_SETPOINT
Definition: optical_flow_landing.c:109
paparazzi.h
OpticalFlowLanding::lp_const
float lp_const
low-pass filter constant
Definition: optical_flow_landing.h:52
OFL_IGAIN
#define OFL_IGAIN
Definition: optical_flow_landing.c:80
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
OpticalFlowLanding::divergence_setpoint
float divergence_setpoint
setpoint for constant divergence approach
Definition: optical_flow_landing.h:54
elc_d_gain_start
float elc_d_gain_start
Definition: optical_flow_landing.c:149
OpticalFlowLanding::pgain_adaptive
float pgain_adaptive
P-gain for adaptive gain control.
Definition: optical_flow_landing.h:67
vertical_ctrl_module_run
void vertical_ctrl_module_run(bool in_flight)
Run the optical flow landing module.
Definition: optical_flow_landing.c:288
istate
float istate
Definition: optical_flow_landing.c:138
telemetry.h
vertical_ctrl_optical_flow_cb
void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
Definition: optical_flow_landing.c:600
OpticalFlowLanding::CONTROL_METHOD
uint32_t CONTROL_METHOD
type of divergence control: 0 = fixed gain, 1 = adaptive gain
Definition: optical_flow_landing.h:64
I
static float I
Definition: trilateration.c:35
OpticalFlowLanding::divergence
float divergence
Divergence estimate.
Definition: optical_flow_landing.h:58
past_divergence_history
float past_divergence_history[OFL_COV_WINDOW_SIZE]
Definition: optical_flow_landing.c:185
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
OFL_AGL_ID
#define OFL_AGL_ID
Definition: optical_flow_landing.c:64
OpticalFlowLanding::elc_oscillate
bool elc_oscillate
Whether or not to oscillate at beginning of elc to find optimum gain.
Definition: optical_flow_landing.h:77
OpticalFlowLanding::igain
float igain
I-gain for constant divergence control.
Definition: optical_flow_landing.h:56
sys_time.h
Architecture independent timing functions.
pprz_stat.h
Statistics functions.
previous_cov_err
float previous_cov_err
Definition: optical_flow_landing.c:142
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
count_covdiv
int32_t count_covdiv
Definition: optical_flow_landing.c:150
OpticalFlowLanding::igain_adaptive
float igain_adaptive
I-gain for adaptive gain control.
Definition: optical_flow_landing.h:68
agl_ev
static abi_event agl_ev
The altitude ABI event.
Definition: optical_flow_landing.c:153
OpticalFlowLanding::previous_err
float previous_err
Previous divergence tracking error.
Definition: optical_flow_landing.h:59
prev_vision_time
float prev_vision_time
Definition: optical_flow_landing.c:140
of_landing_ctrl
struct OpticalFlowLanding of_landing_ctrl
Definition: optical_flow_landing.c:157
autopilot.h
dstate
float dstate
Definition: optical_flow_landing.c:139
D
static float D
Definition: trilateration.c:35
OpticalFlowLanding::d_err
float d_err
difference of error for the D-gain
Definition: optical_flow_landing.h:61
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
thrust_history
float thrust_history[OFL_COV_WINDOW_SIZE]
Definition: optical_flow_landing.c:183
OFL_COV_LANDING_LIMIT
#define OFL_COV_LANDING_LIMIT
Definition: optical_flow_landing.c:105
divergence_setpoint
float divergence_setpoint
Definition: optical_flow_landing.c:144
guidance_v_nominal_throttle
float guidance_v_nominal_throttle
nominal throttle for hover.
Definition: guidance_v.c:108
divergence_vision
float divergence_vision
Definition: optical_flow_landing.c:133
guidance_v_module_run
void guidance_v_module_run(bool in_flight)
Definition: optical_flow_landing.c:627
MINIMUM_GAIN
#define MINIMUM_GAIN
Definition: optical_flow_landing.c:127
lp_cov_div
float lp_cov_div
Definition: optical_flow_landing.c:151
elc_p_gain_start
float elc_p_gain_start
Definition: optical_flow_landing.c:149
optical_flow_landing.h
This module implements optical flow landings in which the divergence is kept constant....
cov_array_filled
uint8_t cov_array_filled
Definition: optical_flow_landing.c:187
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
final_landing_procedure
static uint32_t final_landing_procedure(void)
Execute a final landing procedure.
Definition: optical_flow_landing.c:502
OpticalFlowLanding::dgain
float dgain
D-gain for constant divergence control.
Definition: optical_flow_landing.h:57
OFL_PGAIN
#define OFL_PGAIN
Definition: optical_flow_landing.c:76
stateGetSpeedEnu_f
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
guidance_v_module_enter
void guidance_v_module_enter(void)
Entering the module (user switched to module)
Definition: optical_flow_landing.c:618
elc_phase
int32_t elc_phase
Definition: optical_flow_landing.c:147
OFL_P_LAND_THRESHOLD
#define OFL_P_LAND_THRESHOLD
Definition: optical_flow_landing.c:117
OpticalFlowLanding::sum_err
float sum_err
integration of the error for I-gain
Definition: optical_flow_landing.h:60
OFL_VISION_METHOD
#define OFL_VISION_METHOD
Definition: optical_flow_landing.c:88
OpticalFlowLanding::p_land_threshold
float p_land_threshold
if during the exponential landing the gain reaches this value, the final landing procedure is trigger...
Definition: optical_flow_landing.h:76
OpticalFlowLanding::cov_set_point
float cov_set_point
for adaptive gain control, setpoint of the covariance (oscillations)
Definition: optical_flow_landing.h:65
vision_time
float vision_time
Definition: optical_flow_landing.c:140
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
pused
float pused
Definition: optical_flow_landing.c:137
OpticalFlowLanding::cov_limit
float cov_limit
for fixed gain control, what is the cov limit triggering the landing
Definition: optical_flow_landing.h:66
OpticalFlowLanding::delay_steps
uint32_t delay_steps
number of delay steps for div past
Definition: optical_flow_landing.h:71
OpticalFlowLanding
Definition: optical_flow_landing.h:49
OpticalFlowLanding::pgain
float pgain
P-gain for constant divergence control (from divergence error to thrust)
Definition: optical_flow_landing.h:55
OFL_CONTROL_METHOD
#define OFL_CONTROL_METHOD
Definition: optical_flow_landing.c:92
OpticalFlowLanding::agl
float agl
agl = height from sonar (only used when using "fake" divergence)
Definition: optical_flow_landing.h:50
OFL_LP_CONST
#define OFL_LP_CONST
Definition: optical_flow_landing.c:113
stabilization.h
cov_div
float cov_div
Definition: optical_flow_landing.c:136
OpticalFlowLanding::vel
float vel
vertical velocity as determined with sonar (only used when using "fake" divergence)
Definition: optical_flow_landing.h:53
normalized_thrust
float normalized_thrust
Definition: optical_flow_landing.c:135
reset_all_vars
static void reset_all_vars(void)
Reset all variables:
Definition: optical_flow_landing.c:243
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
OpticalFlowLanding::dgain_adaptive
float dgain_adaptive
D-gain for adaptive gain control.
Definition: optical_flow_landing.h:69
P
static float P[3][3]
Definition: trilateration.c:31
ind_hist
uint32_t ind_hist
Definition: optical_flow_landing.c:186