Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/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,
163  &(of_landing_ctrl.divergence), &divergence_vision_dt, &normalized_thrust,
164  &cov_div, &pstate, &pused, &(of_landing_ctrl.agl));
165 }
166 
169 void vertical_ctrl_agl_cb(uint8_t sender_id, float distance);
170 // Callback function of the optical flow estimate:
171 void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int16_t flow_x,
172  int16_t flow_y, int16_t flow_der_x, int16_t flow_der_y, float quality, float size_divergence, float dist);
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:
236  AbiBindMsgOPTICAL_FLOW(OFL_OPTICAL_FLOW_ID, &optical_flow_ev, vertical_ctrl_optical_flow_cb);
238 }
239 
243 static void reset_all_vars(void)
244 {
246 
247  thrust_set = of_landing_ctrl.nominal_thrust * MAX_PPRZ;
248 
249  cov_div = 0.;
250  normalized_thrust = of_landing_ctrl.nominal_thrust * 100;
251  previous_cov_err = 0.;
252  divergence_vision = 0.;
253  divergence_vision_dt = 0.;
254  divergence_setpoint = 0;
255 
256  vision_time = get_sys_time_float();
257  prev_vision_time = vision_time;
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 
274  pstate = of_landing_ctrl.pgain;
275  pused = pstate;
276  istate = of_landing_ctrl.igain;
277  dstate = of_landing_ctrl.dgain;
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
326  divergence_vision_dt = (divergence_vision / dt);
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;
349  prev_vision_time = vision_time;
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) {
366  thrust_set = final_landing_procedure();
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:
390  if (pstate < of_landing_ctrl.p_land_threshold) {
391  thrust_set = final_landing_procedure();
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:
402  pstate += dt * INCREASE_GAIN_PER_SECOND;
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) {
414  lp_cov_div = of_landing_ctrl.lp_cov_div_factor * lp_cov_div + (1 - of_landing_ctrl.lp_cov_div_factor) * cov_div;
415  }
416  // if oscillating, maintain a counter to see if it endures:
417  if (lp_cov_div <= of_landing_ctrl.cov_set_point) {
418  count_covdiv++;
419  } else {
420  count_covdiv = 0;
421  elc_time_start = get_sys_time_float();
422  }
423  // if the drone has been oscillating long enough, start landing:
425  (count_covdiv > 0 && (get_sys_time_float() - elc_time_start) >= of_landing_ctrl.t_transition)) {
426  // next phase:
427  elc_phase = 1;
428  elc_time_start = get_sys_time_float();
429 
430  // we don't want to oscillate, so reduce the gain:
431  elc_p_gain_start = of_landing_ctrl.reduction_factor_elc * pstate;
432  elc_i_gain_start = of_landing_ctrl.reduction_factor_elc * istate;
433  elc_d_gain_start = of_landing_ctrl.reduction_factor_elc * dstate;
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:
439  pstate = elc_p_gain_start;
440  pused = pstate;
441  istate = elc_i_gain_start;
442  dstate = elc_d_gain_start;
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;
455  elc_time_start = get_sys_time_float();
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:
480  thrust_set = PID_divergence_control(of_landing_ctrl.divergence_setpoint, pused, istate, dstate, dt);
481 
482  // when to make the final landing:
483  if (pstate < of_landing_ctrl.p_land_threshold) {
484  elc_phase = 3;
485  }
486  } else {
487  thrust_set = final_landing_procedure();
488  }
489  }
490 
491  if (in_flight) {
492  Bound(thrust_set, 0.25 * of_landing_ctrl.nominal_thrust * MAX_PPRZ, MAX_PPRZ);
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:
521  divergence_history[ind_hist] = of_landing_ctrl.divergence;
522 
523  normalized_thrust = (float)(thrust / (MAX_PPRZ / 100));
524  thrust_history[ind_hist] = normalized_thrust;
525 
526  int ind_past = ind_hist - of_landing_ctrl.delay_steps;
527  while (ind_past < 0) { ind_past += of_landing_ctrl.window_size; }
528  past_divergence_history[ind_hist] = divergence_history[ind_past];
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
534  cov_div = covariance_f(thrust_history, divergence_history, of_landing_ctrl.window_size);
535  } else if (of_landing_ctrl.COV_METHOD == 1 && cov_array_filled > 1){
536  // todo: delay steps should be invariant to the run frequency
537  cov_div = covariance_f(past_divergence_history, divergence_history, of_landing_ctrl.window_size);
538  }
539 
540  if (cov_array_filled < 2 && ind_hist + 1 == of_landing_ctrl.window_size) {
541  cov_array_filled++;
542  }
543  ind_hist = (ind_hist + 1) % of_landing_ctrl.window_size;
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
567  + D * of_landing_ctrl.d_err) * MAX_PPRZ;
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, float distance)
596 {
597  of_landing_ctrl.agl = distance;
598 }
599 
600 void vertical_ctrl_optical_flow_cb(uint8_t sender_id UNUSED, uint32_t stamp, int16_t flow_x UNUSED,
601  int16_t flow_y UNUSED,
602  int16_t flow_der_x UNUSED, int16_t flow_der_y UNUSED, float quality UNUSED, float size_divergence, float dist UNUSED)
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;
624  thrust_set = of_landing_ctrl.nominal_thrust * MAX_PPRZ;
625 }
626 
627 void guidance_v_module_run(bool in_flight)
628 {
629  vertical_ctrl_module_run(in_flight);
630 }
float previous_err
Previous divergence tracking error.
float d_err
difference of error for the D-gain
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
void guidance_v_module_enter(void)
Entering the module (user switched to module)
float vision_time
int32_t thrust_set
This module implements optical flow landings in which the divergence is kept constant.
float igain
I-gain for constant divergence control.
float thrust_history[OFL_COV_WINDOW_SIZE]
Adaptation block of the vertical guidance.
float elc_p_gain_start
float pstate
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...
float dstate
uint32_t ind_hist
#define OFL_OPTICAL_FLOW_ID
float divergence_vision
float lp_const
low-pass filter constant
#define INCREASE_GAIN_PER_SECOND
void guidance_v_module_run(bool in_flight)
struct OpticalFlowLanding of_landing_ctrl
#define OFL_COV_SETPOINT
Periodic telemetry system header (includes downlink utility and generated code).
float previous_cov_err
float normalized_thrust
float lp_cov_div
#define OFL_AGL_ID
float t_transition
how many seconds the drone has to be oscillating in order to transition to the hover phase with reduc...
float pused
float dgain
D-gain for constant divergence control.
float divergence
Divergence estimate.
Main include for ABI (AirBorneInterface).
static float P[3][3]
Definition: trilateration.c:31
float guidance_v_nominal_throttle
nominal throttle for hover.
Definition: guidance_v.c:104
bool landing
bool elc_oscillate
Whether or not to oscillate at beginning of elc to find optimum gain.
#define OFL_COV_LANDING_LIMIT
float agl
agl = height from sonar (only used when using "fake" divergence)
float igain_adaptive
I-gain for adaptive gain control.
static float I
Definition: trilateration.c:35
static uint32_t final_landing_procedure(void)
Execute a final landing procedure.
#define OFL_VISION_METHOD
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
#define OFL_COV_METHOD
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
float divergence_setpoint
float p_land_threshold
if during the exponential landing the gain reaches this value, the final landing procedure is trigger...
static abi_event agl_ev
The altitude ABI event.
#define OFL_ELC_OSCILLATE
uint8_t cov_array_filled
static float D
Definition: trilateration.c:35
float pgain_adaptive
P-gain for adaptive gain control.
uint32_t VISION_METHOD
whether to use vision (1) or Optitrack / sonar (0)
float past_divergence_history[OFL_COV_WINDOW_SIZE]
float vel
vertical velocity as determined with sonar (only used when using "fake" divergence) ...
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.
int32_t count_covdiv
static void update_errors(float error, float dt)
Updates the integral and differential errors for PID control and sets the previous error...
void vertical_ctrl_module_init(void)
Initialize the optical flow landing module.
float elc_d_gain_start
float elc_i_gain_start
Architecture independent timing functions.
static void reset_all_vars(void)
Reset all variables:
float reduction_factor_elc
reduction factor - after oscillation, how much to reduce the gain...
float divergence_setpoint
setpoint for constant divergence approach
#define OFL_IGAIN
unsigned long uint32_t
Definition: types.h:18
void vertical_ctrl_agl_cb(uint8_t sender_id, float distance)
Function definitions Callback function of the ground altitude.
signed short int16_t
Definition: types.h:17
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
float prev_vision_time
uint32_t elc_time_start
#define OFL_COV_WINDOW_SIZE
#define OFL_CONTROL_METHOD
#define MINIMUM_GAIN
signed long int32_t
Definition: types.h:19
void vertical_ctrl_module_run(bool in_flight)
Run the optical flow landing module.
#define OFL_LP_CONST
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
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:132
int32_t elc_phase
Core autopilot interface common to all firmwares.
#define OFL_DGAIN
static abi_event optical_flow_ev
void guidance_v_module_init(void)
void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int16_t flow_x, int16_t flow_y, int16_t flow_der_x, int16_t flow_der_y, float quality, float size_divergence, float dist)
float agl_lp
low-pass version of agl
unsigned char uint8_t
Definition: types.h:14
static void send_divergence(struct transport_tx *trans, struct link_device *dev)
uint32_t CONTROL_METHOD
type of divergence control: 0 = fixed gain, 1 = adaptive gain
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
float z
in meters
float divergence_history[OFL_COV_WINDOW_SIZE]
Statistics functions.
General stabilization interface for rotorcrafts.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
Common flight_plan functions shared between fixedwing and rotorcraft.
float cov_limit
for fixed gain control, what is the cov limit triggering the landing
float cov_div
uint32_t COV_METHOD
method to calculate the covariance: between thrust and div (0) or div and div past (1) ...
float divergence_vision_dt
float cov_set_point
for adaptive gain control, setpoint of the covariance (oscillations)
uint32_t window_size
number of time steps in "window" used for getting the covariance
#define OFL_P_LAND_THRESHOLD
uint32_t delay_steps
number of delay steps for div past
float pgain
P-gain for constant divergence control (from divergence error to thrust)
float lp_cov_div_factor
low-pass factor for the covariance of divergence in order to trigger the second landing phase in the ...
#define MAX_PPRZ
Definition: paparazzi.h:8
float istate
#define OFL_PGAIN
float sum_err
integration of the error for I-gain
float dgain_adaptive
D-gain for adaptive gain control.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
float nominal_thrust
nominal thrust around which the PID-control operates