Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
optical_flow_hover.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_hover.h"
22 #include "optical_flow_functions.h"
23 
24 #include "generated/airframe.h"
25 #include "paparazzi.h"
26 #include "modules/core/abi.h"
30 #include <stdio.h>
31 
33 //
35 #include "mcu_periph/sys_time.h"
36 //
37 // Additional math functions
38 #include "math/pprz_stat.h"
39 
40 /* Use optical flow estimates */
41 #ifndef OFH_OPTICAL_FLOW_ID
42 #define OFH_OPTICAL_FLOW_ID ABI_BROADCAST
43 #endif
44 PRINT_CONFIG_VAR(OFH_OPTICAL_FLOW_ID)
45 
46 // Set the standard method (1) to apply the algorithm to all 3 axes at the same time
47 #ifndef OFH_HOVER_METHOD
48 #define OFH_HOVER_METHOD 1
49 #endif
50 
51 // Set the standard method to assume the quadrotor is not (0) symmetrical
52 #ifndef XY_SYMMETRICAL
53 #define XY_SYMMETRICAL 0
54 #endif
55 
56 // Set the standard method to use vision signal and the known input and signal (0) optionally one can use the autocovariance (1)
57 #ifndef OFH_COV_METHOD
58 #define OFH_COV_METHOD 0
59 #endif
60 
61 // number of time steps used for calculating the covariance (oscillations) and the delay steps
62 #ifndef COV_WINDOW_SIZE
63 #define COV_WINDOW_SIZE (10*30)
64 #endif
65 
66 // In case the autocovariance is used, select half the window size as the delay
67 #ifndef OF_COV_DELAY_STEPS
68 #define OF_COV_DELAY_STEPS COV_WINDOW_SIZE/2
69 #endif
70 
71 // The low pass filter constant for updating the vision measurements in the algorithm
72 #ifndef OF_LP_CONST
73 #define OF_LP_CONST 0.5
74 #endif
75 
76 // Whether the algorithm should be applied to the phi angle(1) or not(0)
77 #ifndef OFH_OSCPHI
78 #define OFH_OSCPHI 1
79 #endif
80 
81 // Whether the algorithm should be applied to the theta angle(1) or not(0)
82 #ifndef OFH_OSCTHETA
83 #define OFH_OSCTHETA 1
84 #endif
85 
86 // Use default PID gains
87 #ifndef OFH_PGAINZ
88 #define OFH_PGAINZ 0.4
89 #endif
90 
91 #ifndef OFH_IGAINZ
92 #define OFH_IGAINZ 0.f
93 #endif
94 
95 #ifndef OFH_DGAINZ
96 #define OFH_DGAINZ 0.0
97 #endif
98 
99 // Default slope at which the gain is increased
100 #ifndef OFH_RAMPZ
101 #define OFH_RAMPZ 0.15
102 #endif
103 
104 // Default reduction factor to stabilize the quad after oscillating
105 #ifndef OFH_REDUCTIONZ
106 #define OFH_REDUCTIONZ 0.45
107 #endif
108 
109 // The threshold value for the covariance oscillation measurement
110 #ifndef OFH_COVDIV_SETPOINT
111 #define OFH_COVDIV_SETPOINT -0.02
112 #endif
113 
114 // Use default PID gains
115 #ifndef OFH_PGAINX
116 #define OFH_PGAINX 0.f
117 #endif
118 
119 #ifndef OFH_IGAINX
120 #define OFH_IGAINX 0.00002
121 #endif
122 
123 #ifndef OFH_DGAINX
124 #define OFH_DGAINX 0.f
125 #endif
126 
127 #ifndef OFH_PGAINY
128 #define OFH_PGAINY 0.f
129 #endif
130 
131 #ifndef OFH_IGAINY
132 #define OFH_IGAINY 0.00002
133 #endif
134 
135 #ifndef OFH_DGAINY
136 #define OFH_DGAINY 0.f
137 #endif
138 
139 // Default slope at which the gain is increased
140 #ifndef OFH_RAMPXY
141 #define OFH_RAMPXY 0.0008
142 #endif
143 
144 // Default reduction factor to stabilize the quad after oscillating
145 #ifndef OFH_REDUCTIONXY
146 #define OFH_REDUCTIONXY 0.3
147 #endif
148 
149 // The threshold value for the covariance oscillation measurement
150 #ifndef OFH_COVFLOW_SETPOINT
151 #define OFH_COVFLOW_SETPOINT -500.f
152 #endif
153 
154 // The default slopes for the height-gain relationships
155 #ifndef OFH_VER_SLOPE_A
156 #define OFH_VER_SLOPE_A 0.5
157 #endif
158 
159 #ifndef OFH_VER_SLOPE_B
160 #define OFH_VER_SLOPE_B 0.25
161 #endif
162 
163 #ifndef OFH_HOR_X_SLOPE_A
164 #define OFH_HOR_X_SLOPE_A 2.f
165 #endif
166 
167 #ifndef OFH_HOR_X_SLOPE_B
168 #define OFH_HOR_X_SLOPE_B 0.5
169 #endif
170 
171 #ifndef OFH_HOR_Y_SLOPE_A
172 #define OFH_HOR_Y_SLOPE_A OFH_HOR_X_SLOPE_A
173 #endif
174 
175 #ifndef OFH_HOR_Y_SLOPE_B
176 #define OFH_HOR_Y_SLOPE_B OFH_HOR_X_SLOPE_B
177 #endif
178 
179 
182 
183 // variables retained between module calls
185 
192 
193 // Stabilizing commands
195 
198 
200 
201 // The optical flow ABI event
203 
204 // struct containing most relevant parameters
208 
209 // variables used in the GCS
210 bool oscphi;
211 bool osctheta;
215 
216 
218 // Callback function of the optical flow estimate:
219 void ofh_optical_flow_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp, int32_t flow_x, int32_t flow_y,
220  int32_t flow_der_x, int32_t flow_der_y, float quality, float size_div);
221 
222 // resetting all variables to be called for instance when starting up / re-entering module
223 static void reset_horizontal_vars(void);
224 static void reset_vertical_vars(void);
225 void vertical_ctrl_module_init(void);
226 void vertical_ctrl_module_run(bool in_flight);
227 void horizontal_ctrl_module_init(void);
228 void horizontal_ctrl_module_run(bool in_flight);
229 
230 // Compute OptiTrack stabilization for 1/2 axes
231 void computeOptiTrack(bool phi, bool theta, struct Int32Eulers *opti_sp_eu);
232 #ifndef GH_GAIN_SCALE
233 #define GH_GAIN_SCALE 2
234 #endif
235 #ifndef MAX_POS_ERR
236 #define MAX_POS_ERR POS_BFP_OF_REAL(16.)
237 #endif
238 #ifndef MAX_SPEED_ERR
239 #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
240 #endif
246 
247 
248 // sending the divergence message to the ground station:
249 static void send_optical_flow_hover(struct transport_tx *trans, struct link_device *dev)
250 {
251  pprz_msg_send_OPTICAL_FLOW_HOVER(trans, dev, AC_ID, &of_hover.flowX, &of_hover.flowY, &of_hover.divergence,
255 }
256 
257 
258 // Init the optical flow hover module
260 {
261  of_hover_ctrl_X.setpoint = 0.0f;
268 
269  of_hover_ctrl_Y.setpoint = 0.0f;
276 
277  of_hover_ctrl_Z.setpoint = 0.0f;
284 
285  oscphi = OFH_OSCPHI;
287 
290 
293 
294  // Subscribe to the optical flow estimator:
295  AbiBindMsgOPTICAL_FLOW(OFH_OPTICAL_FLOW_ID, &optical_flow_ev, ofh_optical_flow_cb);
296 
297  // register telemetry:
298  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTICAL_FLOW_HOVER, send_optical_flow_hover);
299 }
300 
305 {
306  // filling the of_hover_ctrl struct with default values:
308 }
309 
314 {
315  // filling the of_hover_ctrl struct with default values:
317 }
318 
322 static void reset_horizontal_vars(void)
323 {
324  // Take the current angles as neutral
325  struct Int32Eulers tempangle;
329 
330  des_inputs.phi = 0;
331  des_inputs.theta = 0;
332 
334  // Z - X - Y Order
335  oscillatingX = 1;
336  oscillatingY = 1;
340  // Z Set XY
341  oscillatingX = 1;
342  oscillatingY = 1;
345  of_hover_ctrl_X.PID.I = OFH_IGAINX / 4; // Have a slighly lower I gain during Z
346  of_hover_ctrl_Y.PID.I = OFH_IGAINY / 4; // Have a slighly lower I gain during Z
347  } else {
348  // if V is in NAV or hover_method = 1
349  //All axes
350  oscillatingX = 0;
351  oscillatingY = 0;
354  }
355 
356  flowX = 0;
357  flowY = 0;
358 
359  of_hover_ctrl_X.PID.sum_err = 0.0f;
360  of_hover_ctrl_X.PID.d_err = 0.0f;
362 
363  of_hover_ctrl_Y.PID.sum_err = 0.0f;
364  of_hover_ctrl_Y.PID.d_err = 0.0f;
366 
369 
370  ind_histXY = 0;
371  cov_array_filledXY = 0;
372 
373  covariances.x = 0.0f;
374  covariances.y = 0.0f;
375 
376  for (uint16_t i = 0; i < COV_WINDOW_SIZE; i++) {
377  historyX.OF[i] = 0.0f;
378  historyY.OF[i] = 0.0f;
379  historyX.past_OF[i] = 0.0f;
380  historyY.past_OF[i] = 0.0f;
381  historyX.input[i] = 0.0f;
382  historyY.input[i] = 0.0f;
383  }
384 
385  of_hover.flowX = 0;
386  of_hover.flowY = 0;
387 
390 }
391 
395 static void reset_vertical_vars(void)
396 {
397  oscillatingZ = 0;
398 
399  divergence_vision = 0;
400  of_hover.divergence = 0;
401 
402  for (uint16_t i = 0; i < COV_WINDOW_SIZE; i++) {
403  historyZ.OF[i] = 0.0f;
404  historyZ.past_OF[i] = 0.0f;
405  historyZ.input[i] = 0.0f;
406  }
407 
408  ind_histZ = 0;
409 
410  covariances.z = 0.0f;
411  cov_array_filledZ = 0;
412 
414 
415  of_hover_ctrl_Z.PID.sum_err = 0.0f;
416  of_hover_ctrl_Z.PID.d_err = 0.0f;
418 
421 }
422 
423 // Read H RC
425 
429 void horizontal_ctrl_module_run(bool in_flight)
430 {
431  /***********
432  * TIME
433  ***********/
434  float ventral_factor = -1.28f; // magic number comprising field of view etc.
435 
436  float dt = vision_time - prev_vision_timeXY;
437 
438  // check if new measurement received
439  if (dt <= 1e-5f) {
440  return;
441  }
442 
443  /***********
444  * VISION
445  ***********/
446 
447  float lp_factor = dt / OF_LP_CONST;
448  Bound(lp_factor, 0.f, 1.f);
449 
450  float new_flowX = (flowX * ventral_factor) / dt;
451  float new_flowY = (flowY * ventral_factor) / dt;
452 
453  //TODO: deal with (unlikely) fast changes in Flow?
454 
455  // low-pass filter the divergence:
456  of_hover.flowX += (new_flowX - of_hover.flowX) * lp_factor;
457  of_hover.flowY += (new_flowY - of_hover.flowY) * lp_factor;
458 
459 
460  /***********
461  * CONTROL
462  ***********/
463 
464  if (!oscillatingX) {
465  // if not oscillating, increase gain
467  }
468  if (!oscillatingY) {
469  // if not oscillating, increase gain
471  }
472 
473  // set desired pitch en roll
474  if (oscphi) {
477  }
478  if (osctheta) {
481  }
482 
483  // update covariance
487 
488  // Check for oscillations
490  oscillatingX = 1;
491 
492  if (hover_method == 0) {
493  //Start the Y axis
494  oscillatingY = 0;
496  }
498 
499  if (XY_SYMMETRICAL) {
500  // also set Y
501  oscillatingY = 1;
503  }
504  }
506  oscillatingY = 1;
508  }
509 
510  // Compute 0, 1 or 2 horizontal axes with optitrack
512  // Run the stabilization mode
514 
516 }
517 
521 void vertical_ctrl_module_run(bool in_flight)
522 {
523  /***********
524  * TIME
525  ***********/
526 
527  float div_factor; // factor that maps divergence in pixels as received from vision to 1 / frame
528 
529  float dt = vision_time - prev_vision_timeZ;
530 
531  // check if new measurement received
532  if (dt <= 1e-5f) {
533  return;
534  }
535 
536  /***********
537  * VISION
538  ***********/
539 
540  float lp_factor = dt / OF_LP_CONST;
541  Bound(lp_factor, 0.f, 1.f);
542 
543  // Vision
544  div_factor = -1.28f; // magic number comprising field of view etc.
545  float new_divergence = (divergence_vision * div_factor) / dt;
546 
547  // deal with (unlikely) fast changes in divergence:
548  static const float max_div_dt = 0.20f;
549  if (fabsf(new_divergence - of_hover.divergence) > max_div_dt) {
550  if (new_divergence < of_hover.divergence) { new_divergence = of_hover.divergence - max_div_dt; }
551  else { new_divergence = of_hover.divergence + max_div_dt; }
552  }
553  // low-pass filter the divergence:
554 
555  of_hover.divergence += (new_divergence - of_hover.divergence) * lp_factor;
557 
558  /***********
559  * CONTROL
560  ***********/
561  if (!oscillatingZ) {
562  // if not oscillating, increase gain
564  }
565 
566  // use the divergence for control:
569 
571 
572  // Check for oscillations
574  float estimatedHeight = of_hover_ctrl_Z.PID.P * OFH_VER_SLOPE_A + OFH_VER_SLOPE_B; // Vertical slope Z
575  oscillatingZ = 1;
576  of_hover_ctrl_Z.setpoint = 0.0f;
578 
579  if (hover_method == 0) {
580  // Z- X - Y Order
581  // Start the X axis
582  oscillatingX = 0;
584  } else if (hover_method == 2) {
585  // Start XY axes with computed slope
586  of_hover_ctrl_X.PID.sum_err = 0.0f;
587  of_hover_ctrl_Y.PID.sum_err = 0.0f;
590  of_hover_ctrl_X.PID.P = OFH_REDUCTIONXY * (estimatedHeight * OFH_HOR_X_SLOPE_A - OFH_HOR_X_SLOPE_B); // Horizontal Slope X
591  of_hover_ctrl_Y.PID.P = OFH_REDUCTIONXY * (estimatedHeight * OFH_HOR_Y_SLOPE_A - OFH_HOR_Y_SLOPE_B); // Horizontal Slope Y
592  } else {
593  // hover_method = 1
594  //All axes
595  }
596  }
597 
598  stabilization_cmd[COMMAND_THRUST] = des_inputs.thrust;
599 }
600 
601 void ofh_optical_flow_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp, int32_t flow_x, int32_t flow_y,
602  int32_t flow_der_x, int32_t flow_der_y, float quality, float size_div)
603 {
604  if (!derotated) {
605  flowX = flow_x;
606  flowY = flow_y;
607  } else {
608  flowX = flow_der_x;
609  flowY = flow_der_y;
610  }
611 
612  divergence_vision = size_div;
613 
614  vision_time = ((float)stamp) / 1e6;
615 }
616 
618 // Call our vertical controller
620 {
622 }
623 
624 // Call our horizontal controller
626 {
628 }
629 
634 {
636 
637  // adaptive estimation - assume hover condition when entering the module
638  of_hover_ctrl_Z.nominal_value = (float) stabilization_cmd[COMMAND_THRUST] / MAX_PPRZ;
640 }
641 
646 {
647  // Set current psi as heading
649 
652 
653 }
654 
655 // Run the veritcal controller
656 void guidance_v_module_run(bool in_flight)
657 {
658  if (electrical.bat_low) {
660  } else {
661  // your vertical controller goes here
662  vertical_ctrl_module_run(in_flight);
663  }
664 }
665 
666 // Run the horizontal controller
667 void guidance_h_module_run(bool in_flight)
668 {
669 
670  if (electrical.bat_low) {
672  } else {
673  horizontal_ctrl_module_run(in_flight);
674  stabilization_attitude_run(in_flight);
675  }
676 }
677 
684 void computeOptiTrack(bool phi, bool theta, struct Int32Eulers *opti_sp_eu)
685 {
686 
687  bool optiVelOnly;
688  optiVelOnly = 0;
689 
691 
692  struct NedCoor_i vel_from_GPS;
693  struct NedCoor_i pos_from_GPS;
694 
695  vel_from_GPS = *stateGetSpeedNed_i();
696  pos_from_GPS = *stateGetPositionNed_i();
697 
698  /* maximum bank angle: default 20 deg, max 40 deg*/
699  static const int32_t traj_max_bank = Min(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC),
700  BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC));
701  static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC);
702 
703  /* compute position error */
705  /* saturate it */
707 
708  struct Int32Vect2 ref_speed;
709  ref_speed.x = 0;
710  ref_speed.y = 0;
711 
712  /* compute speed error */
713  VECT2_DIFF(of_hover_speed_err, ref_speed, vel_from_GPS);
714  /* saturate it */
716 
717  if (optiVelOnly) {
718  of_hover_pos_err.x = 0;
719  of_hover_pos_err.y = 0;
720  }
721 
722  /* run PID */
724  ((GUIDANCE_H_PGAIN * of_hover_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
725  ((GUIDANCE_H_DGAIN * (of_hover_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
727  ((GUIDANCE_H_PGAIN * of_hover_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
728  ((GUIDANCE_H_DGAIN * (of_hover_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
729 
730  /* trim max bank angle from PD */
731  VECT2_STRIM(of_hover_cmd_earth, -traj_max_bank, traj_max_bank);
732 
733  of_hover_trim_att_integrator.x += (GUIDANCE_H_IGAIN * of_hover_cmd_earth.x);
734  of_hover_trim_att_integrator.y += (GUIDANCE_H_IGAIN * of_hover_cmd_earth.y);
735  /* saturate it */
737  (traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2)));
738 
739  /* add it to the command */
742 
743  VECT2_STRIM(of_hover_cmd_earth, -total_max_bank, total_max_bank);
744 
745  // Compute Angle Setpoints - Taken from Stab_att_quat
746  int32_t s_psi, c_psi;
747  PPRZ_ITRIG_SIN(s_psi, psi);
748  PPRZ_ITRIG_COS(c_psi, psi);
749 
750  if (phi) {
751  opti_sp_eu->phi = (-s_psi * of_hover_cmd_earth.x + c_psi * of_hover_cmd_earth.y) >> INT32_TRIG_FRAC;
752  }
753  if (theta) {
754  opti_sp_eu->theta = -(c_psi * of_hover_cmd_earth.x + s_psi * of_hover_cmd_earth.y) >> INT32_TRIG_FRAC;
755  }
756 }
GainsPID::err
float err
Current tracking error.
Definition: optical_flow_functions.h:31
vision_time
float vision_time
Definition: optical_flow_hover.c:184
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
ind_histZ
uint32_t ind_histZ
Definition: optical_flow_functions.c:43
electrical.h
of_hover_ctrl_Z
struct OpticalFlowHoverControl of_hover_ctrl_Z
Definition: optical_flow_hover.c:207
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
des_inputs
struct DesiredInputs des_inputs
Definition: optical_flow_hover.c:180
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
optical_flow_ev
static abi_event optical_flow_ev
Definition: optical_flow_hover.c:202
GainsPID::D
float D
D-gain for control.
Definition: optical_flow_functions.h:30
oscillatingX
bool oscillatingX
Definition: optical_flow_hover.c:186
OFH_COV_METHOD
#define OFH_COV_METHOD
Definition: optical_flow_hover.c:58
computeOptiTrack
void computeOptiTrack(bool phi, bool theta, struct Int32Eulers *opti_sp_eu)
Get the desired Euler angles for optitrack stabilization.
Definition: optical_flow_hover.c:684
stabilization_attitude.h
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
GUIDANCE_H_MAX_BANK
#define GUIDANCE_H_MAX_BANK
Definition: guidance_h.c:70
INT32_SPEED_FRAC
#define INT32_SPEED_FRAC
Definition: pprz_algebra_int.h:73
abi.h
OFH_DGAINY
#define OFH_DGAINY
Definition: optical_flow_hover.c:136
OFH_DGAINZ
#define OFH_DGAINZ
Definition: optical_flow_hover.c:96
oscillatingY
bool oscillatingY
Definition: optical_flow_hover.c:187
ofh_optical_flow_cb
void ofh_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_div)
Function definitions.
Definition: optical_flow_hover.c:601
OFH_HOR_X_SLOPE_B
#define OFH_HOR_X_SLOPE_B
Definition: optical_flow_hover.c:168
get_sys_time_float
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
DesiredInputs::thrust
int32_t thrust
Definition: optical_flow_functions.h:65
of_hover_speed_err
struct Int32Vect2 of_hover_speed_err
Definition: optical_flow_hover.c:242
INT32_ANGLE_FRAC
#define INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:116
GUIDANCE_V_MODE_MODULE
#define GUIDANCE_V_MODE_MODULE
Definition: guidance_v.h:41
OpticalFlowHoverControl::PID
struct GainsPID PID
The struct with the PID gains.
Definition: optical_flow_functions.h:44
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
OFH_REDUCTIONXY
#define OFH_REDUCTIONXY
Definition: optical_flow_hover.c:146
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
ofh_sp_eu
struct Int32Eulers ofh_sp_eu
Definition: optical_flow_hover.c:194
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
OFH_PGAINY
#define OFH_PGAINY
Definition: optical_flow_hover.c:128
PPRZ_ITRIG_COS
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:112
cov_array_filledZ
uint8_t cov_array_filledZ
Definition: optical_flow_functions.c:44
hover_method
uint8_t hover_method
Method used to hover 0 = All axis after each other; 1 = all axis at the same time; 2 = vertical only,...
Definition: optical_flow_hover.c:214
DesiredInputs
Definition: optical_flow_functions.h:62
reset_horizontal_vars
static void reset_horizontal_vars(void)
Reset all horizontal variables:
Definition: optical_flow_hover.c:322
OFH_COVDIV_SETPOINT
#define OFH_COVDIV_SETPOINT
Definition: optical_flow_hover.c:111
stateGetPositionNed_i
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
covariances
struct FloatVect3 covariances
Definition: optical_flow_hover.c:181
guidance_h_module_init
void guidance_h_module_init(void)
Definition: optical_flow_hover.c:625
int32_eulers_of_quat
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
Definition: pprz_algebra_int.c:523
paparazzi.h
OFhistory::input
float input[COV_WINDOW_SIZE]
Definition: optical_flow_functions.h:38
guidance_h_module_read_rc
void guidance_h_module_read_rc(void)
Definition: optical_flow_hover.c:424
GainsPID::I
float I
I-gain for control.
Definition: optical_flow_functions.h:29
guidance_v_module_init
void guidance_v_module_init(void)
Definition: optical_flow_hover.c:619
OFH_VER_SLOPE_B
#define OFH_VER_SLOPE_B
Definition: optical_flow_hover.c:160
GainsPID::previous_err
float previous_err
Previous tracking error.
Definition: optical_flow_functions.h:32
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
historyY
struct OFhistory historyY
Definition: optical_flow_hover.c:191
VECT2_COPY
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
VECT2_STRIM
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:110
FloatVect3
Definition: pprz_algebra_float.h:54
telemetry.h
OpticalFlowHoverControl::ramp
float ramp
The ramp pused is increased with per dt.
Definition: optical_flow_functions.h:48
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
AP_MODE_NAV
#define AP_MODE_NAV
Definition: autopilot_static.h:48
OFH_IGAINX
#define OFH_IGAINX
Definition: optical_flow_hover.c:120
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
OFH_REDUCTIONZ
#define OFH_REDUCTIONZ
Definition: optical_flow_hover.c:106
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
OFH_OSCPHI
#define OFH_OSCPHI
Definition: optical_flow_hover.c:78
stateGetSpeedNed_i
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
Electrical::bat_low
bool bat_low
battery low status
Definition: electrical.h:49
FLOAT_OF_BFP
#define FLOAT_OF_BFP(_vbfp, _frac)
Definition: pprz_algebra_int.h:207
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
prev_vision_timeZ
float prev_vision_timeZ
Definition: optical_flow_hover.c:184
OFH_PGAINX
#define OFH_PGAINX
Definition: optical_flow_hover.c:116
OFH_HOVER_METHOD
#define OFH_HOVER_METHOD
Definition: optical_flow_hover.c:48
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
OFH_RAMPZ
#define OFH_RAMPZ
Definition: optical_flow_hover.c:101
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
oscphi
bool oscphi
Definition: optical_flow_hover.c:210
OFH_PGAINZ
#define OFH_PGAINZ
Definition: optical_flow_hover.c:88
OpticalFlowHoverControl::setpoint
float setpoint
setpoint for constant divergence/flow
Definition: optical_flow_functions.h:52
GH_GAIN_SCALE
#define GH_GAIN_SCALE
Definition: optical_flow_hover.c:233
oscillatingZ
bool oscillatingZ
Definition: optical_flow_hover.c:196
Int32Vect2
Definition: pprz_algebra_int.h:83
OFhistory::OF
float OF[COV_WINDOW_SIZE]
Definition: optical_flow_functions.h:39
sys_time.h
Architecture independent timing functions.
historyZ
struct OFhistory historyZ
Definition: optical_flow_hover.c:199
pprz_stat.h
Statistics functions.
OFH_IGAINZ
#define OFH_IGAINZ
Definition: optical_flow_hover.c:92
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
send_optical_flow_hover
static void send_optical_flow_hover(struct transport_tx *trans, struct link_device *dev)
Definition: optical_flow_hover.c:249
of_hover_trim_att_integrator
struct Int32Vect2 of_hover_trim_att_integrator
Definition: optical_flow_hover.c:244
optical_flow_hover_init
void optical_flow_hover_init()
Definition: optical_flow_hover.c:259
cov_array_filledXY
uint8_t cov_array_filledXY
Definition: optical_flow_functions.c:42
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
stabilization_attitude_run
void stabilization_attitude_run(bool in_flight)
Definition: stabilization_attitude_euler_float.c:176
of_hover_cmd_earth
struct Int32Vect2 of_hover_cmd_earth
Definition: optical_flow_hover.c:245
OpticalFlowHoverControl::reduction_factor
float reduction_factor
Reduce the gain by this factor when oscillating.
Definition: optical_flow_functions.h:50
of_hover_ctrl_X
struct OpticalFlowHoverControl of_hover_ctrl_X
Definition: optical_flow_hover.c:205
flowY
int32_t flowY
Definition: optical_flow_hover.c:189
of_hover_ctrl_Y
struct OpticalFlowHoverControl of_hover_ctrl_Y
Definition: optical_flow_hover.c:206
optical_flow_hover.h
BFP_OF_REAL
#define BFP_OF_REAL(_vr, _frac)
Definition: pprz_algebra_int.h:206
OF_LP_CONST
#define OF_LP_CONST
Definition: optical_flow_hover.c:73
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
of_hover_ref_pos
struct Int32Vect2 of_hover_ref_pos
Definition: optical_flow_hover.c:243
OFH_RAMPXY
#define OFH_RAMPXY
Definition: optical_flow_hover.c:141
historyX
struct OFhistory historyX
Definition: optical_flow_hover.c:190
horizontal_ctrl_module_init
void horizontal_ctrl_module_init(void)
Initialize the horizontal optical flow hover module.
Definition: optical_flow_hover.c:313
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
guidance_v_module_run
void guidance_v_module_run(bool in_flight)
Definition: optical_flow_hover.c:656
autopilot_static_set_mode
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot_static.c:140
guidance_v_module_enter
void guidance_v_module_enter(void)
Entering the vertical module (user switched to module)
Definition: optical_flow_hover.c:633
DesiredInputs::phi
float phi
Definition: optical_flow_functions.h:63
horizontal_ctrl_module_run
void horizontal_ctrl_module_run(bool in_flight)
Run the horizontal optical flow hover module.
Definition: optical_flow_hover.c:429
derotated
bool derotated
Definition: optical_flow_hover.c:212
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
flowX
int32_t flowX
Definition: optical_flow_hover.c:188
OFhistory
Definition: optical_flow_functions.h:37
OFH_HOR_Y_SLOPE_A
#define OFH_HOR_Y_SLOPE_A
Definition: optical_flow_hover.c:172
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
guidance_h_module_run
void guidance_h_module_run(bool in_flight)
Definition: optical_flow_hover.c:667
GUIDANCE_V_MODE_MODULE_SETTING
#define GUIDANCE_V_MODE_MODULE_SETTING
Definition: ctrl_module_innerloop_demo.h:44
optical_flow_functions.h
OFH_VER_SLOPE_A
#define OFH_VER_SLOPE_A
Definition: optical_flow_hover.c:156
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
reset_vertical_vars
static void reset_vertical_vars(void)
Reset all vertical variables:
Definition: optical_flow_hover.c:395
OpticalFlowHoverControl
Definition: optical_flow_functions.h:43
vertical_ctrl_module_run
void vertical_ctrl_module_run(bool in_flight)
Run the vertical optical flow hover module.
Definition: optical_flow_hover.c:521
OFH_DGAINX
#define OFH_DGAINX
Definition: optical_flow_hover.c:124
guidance_h_module_enter
void guidance_h_module_enter(void)
Entering the horizontal module (user switched to module)
Definition: optical_flow_hover.c:645
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
OFH_HOR_X_SLOPE_A
#define OFH_HOR_X_SLOPE_A
Definition: optical_flow_hover.c:164
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
OFH_COVFLOW_SETPOINT
#define OFH_COVFLOW_SETPOINT
Definition: optical_flow_hover.c:151
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
XY_SYMMETRICAL
#define XY_SYMMETRICAL
Definition: optical_flow_hover.c:53
stabilization_attitude_set_rpy_setpoint_i
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Definition: stabilization_attitude_euler_float.c:154
OFH_OPTICAL_FLOW_ID
#define OFH_OPTICAL_FLOW_ID
Definition: optical_flow_hover.c:42
stateGetNedToBodyEulers_i
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
OpticalFlowHover::flowY
float flowY
Flow estimate in Y direction.
Definition: optical_flow_functions.h:59
stab_att_sp_quat
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
Definition: stabilization_attitude_heli_indi.c:127
MAX_SPEED_ERR
#define MAX_SPEED_ERR
Definition: optical_flow_hover.c:239
osctheta
bool osctheta
Definition: optical_flow_hover.c:211
OpticalFlowHoverControl::cov_setpoint
float cov_setpoint
for adaptive gain control, setpoint of the covariance (oscillations)
Definition: optical_flow_functions.h:53
electrical
struct Electrical electrical
Definition: electrical.c:66
stabilization.h
divergence_vision
float divergence_vision
Definition: optical_flow_hover.c:197
vertical_ctrl_module_init
void vertical_ctrl_module_init(void)
Initialize the vertical optical flow hover module.
Definition: optical_flow_hover.c:304
INT32_TRIG_FRAC
#define INT32_TRIG_FRAC
Definition: pprz_algebra_int.h:154
prev_vision_timeXY
float prev_vision_timeXY
Definition: optical_flow_hover.c:184
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
of_hover_pos_err
struct Int32Vect2 of_hover_pos_err
Definition: optical_flow_hover.c:241
ind_histXY
uint32_t ind_histXY
Definition: optical_flow_functions.c:41
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
OFH_OSCTHETA
#define OFH_OSCTHETA
Definition: optical_flow_hover.c:83
INT32_POS_FRAC
#define INT32_POS_FRAC
Definition: pprz_algebra_int.h:68
GainsPID::P
float P
P-gain for control.
Definition: optical_flow_functions.h:28
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
MAX_POS_ERR
#define MAX_POS_ERR
Definition: optical_flow_hover.c:236
OFH_HOR_Y_SLOPE_B
#define OFH_HOR_Y_SLOPE_B
Definition: optical_flow_hover.c:176
Min
#define Min(x, y)
Definition: esc_dshot.c:85
GainsPID::d_err
float d_err
difference of error for the D-gain
Definition: optical_flow_functions.h:34
OFH_IGAINY
#define OFH_IGAINY
Definition: optical_flow_hover.c:132
PPRZ_ITRIG_SIN
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:111