Paparazzi UAS  v5.15_devel-109-gee85905
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/abi.h"
29 #include "subsystems/electrical.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, int16_t flow_x, int16_t flow_y,
220  int16_t flow_der_x, int16_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;
286  osctheta = OFH_OSCTHETA;
287 
288  cov_method = OFH_COV_METHOD;
289  hover_method = OFH_HOVER_METHOD;
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 
333  if ((hover_method == 0) && (GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE)) {
334  // Z - X - Y Order
335  oscillatingX = 1;
336  oscillatingY = 1;
339  } else if ((hover_method == 2) && (GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE)) {
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 
388  vision_time = get_sys_time_float();
389  prev_vision_timeXY = vision_time;
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 
419  vision_time = get_sys_time_float();
420  prev_vision_timeZ = vision_time;
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
485  ofh_sp_eu.phi = BFP_OF_REAL(RadOfDeg(des_inputs.phi * oscphi), INT32_ANGLE_FRAC);
486  ofh_sp_eu.theta = BFP_OF_REAL(RadOfDeg(des_inputs.theta * osctheta), INT32_ANGLE_FRAC);
487 
488  // Check for oscillations
489  if ((covariances.x < of_hover_ctrl_X.cov_setpoint) && (!oscillatingX)) {
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  }
505  if ((covariances.y < of_hover_ctrl_Y.cov_setpoint) && (!oscillatingY)) {
506  oscillatingY = 1;
508  }
509 
510  // Compute 0, 1 or 2 horizontal axes with optitrack
511  computeOptiTrack(!oscphi, !osctheta, &ofh_sp_eu);
512  // Run the stabilization mode
514 
515  prev_vision_timeXY = vision_time;
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;
556  prev_vision_timeZ = vision_time;
557 
558  /***********
559  * CONTROL
560  ***********/
561  if (!oscillatingZ) {
562  // if not oscillating, increase gain
564  }
565 
566  // use the divergence for control:
569 
570  covariances.z = set_cov_div(cov_method, &historyZ, &des_inputs);
571 
572  // Check for oscillations
573  if (covariances.z < of_hover_ctrl_Z.cov_setpoint && (!oscillatingZ)) {
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, int16_t flow_x, int16_t flow_y,
602  int16_t flow_der_x, int16_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 }
int32_t psi
in rad with INT32_ANGLE_FRAC
void guidance_h_module_run(bool in_flight)
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
unsigned short uint16_t
Definition: types.h:16
#define OFH_IGAINX
void ofh_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_div)
Function definitions.
struct OpticalFlowHoverControl of_hover_ctrl_Z
#define OFH_VER_SLOPE_B
#define Min(x, y)
Definition: esc_dshot.c:85
float ramp
The ramp pused is increased with per dt.
#define GUIDANCE_H_MAX_BANK
Definition: guidance_h.c:66
#define INT32_SPEED_FRAC
bool osctheta
Periodic telemetry system header (includes downlink utility and generated code).
#define OFH_COV_METHOD
General attitude stabilization interface for rotorcrafts.
void vertical_ctrl_module_run(bool in_flight)
Run the vertical optical flow hover module.
#define OFH_HOR_X_SLOPE_B
int32_t theta
in rad with INT32_ANGLE_FRAC
uint32_t ind_histZ
float past_OF[COV_WINDOW_SIZE]
#define INT32_ANGLE_FRAC
void guidance_v_module_init(void)
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:110
bool derotated
float prev_vision_timeZ
struct OpticalFlowHoverControl of_hover_ctrl_X
float previous_err
Previous tracking error.
#define OFH_HOR_Y_SLOPE_A
#define OFH_OSCPHI
float vision_time
int16_t flowX
Main include for ABI (AirBorneInterface).
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
struct Int32Eulers ofh_sp_eu
struct Int32Vect2 of_hover_trim_att_integrator
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
struct OFhistory historyZ
void stabilization_attitude_run(bool in_flight)
#define FLOAT_OF_BFP(_vbfp, _frac)
#define OFH_DGAINY
bool oscillatingY
#define OFH_RAMPZ
#define OFH_HOR_Y_SLOPE_B
struct GainsPID PID
The struct with the PID gains.
float I
I-gain for control.
bool oscillatingZ
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
#define GUIDANCE_V_MODE_MODULE_SETTING
#define MAX_POS_ERR
void optical_flow_hover_init()
void guidance_h_module_enter(void)
Entering the horizontal module (user switched to module)
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
#define GH_GAIN_SCALE
struct OpticalFlowHover of_hover
float cov_setpoint
for adaptive gain control, setpoint of the covariance (oscillations)
bool oscillatingX
#define BFP_OF_REAL(_vr, _frac)
#define PPRZ_ITRIG_SIN(_s, _a)
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
#define GUIDANCE_V_MODE_MODULE
Definition: guidance_v.h:41
#define OFH_REDUCTIONXY
#define OFH_DGAINX
float sum_err
integration of the error for I-gain
Interface for electrical status: supply voltage, current, battery status, etc.
Architecture independent timing functions.
void horizontal_ctrl_module_init(void)
Initialize the horizontal optical flow hover module.
bool cov_method
method to calculate the covariance: between thrust and div / angle and flow (0) or div and div past /...
#define OFH_COVFLOW_SETPOINT
euler angles
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
#define OFH_HOR_X_SLOPE_A
#define XY_SYMMETRICAL
float divergence_vision
unsigned long uint32_t
Definition: types.h:18
struct Int32Vect2 of_hover_speed_err
uint8_t hover_method
Method used to hover 0 = All axis after each other; 1 = all axis at the same time; 2 = vertical only...
signed short int16_t
Definition: types.h:17
#define OFH_OPTICAL_FLOW_ID
float flowY
Flow estimate in Y direction.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
struct DesiredInputs des_inputs
void guidance_h_module_read_rc(void)
float reduction_factor
Reduce the gain by this factor when oscillating.
void guidance_v_module_run(bool in_flight)
#define OFH_REDUCTIONZ
#define INT32_POS_FRAC
float input[COV_WINDOW_SIZE]
float err
Current tracking error.
#define COV_WINDOW_SIZE
#define OFH_PGAINZ
float prev_vision_timeXY
bool bat_low
battery low status
Definition: electrical.h:49
struct FloatVect3 covariances
struct OpticalFlowHoverControl of_hover_ctrl_Y
#define OFH_COVDIV_SETPOINT
#define OFH_VER_SLOPE_A
signed long int32_t
Definition: types.h:19
struct Int32Vect2 of_hover_pos_err
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define INT32_TRIG_FRAC
#define OFH_DGAINZ
float flowX
Flow estimate in X direction.
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
float setpoint
setpoint for constant divergence/flow
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]
int32_t phi
in rad with INT32_ANGLE_FRAC
#define AP_MODE_NAV
#define OFH_PGAINY
unsigned char uint8_t
Definition: types.h:14
uint8_t cov_array_filledZ
vector in North East Down coordinates
void guidance_h_module_init(void)
struct Int32Vect2 of_hover_cmd_earth
void computeOptiTrack(bool phi, bool theta, struct Int32Eulers *opti_sp_eu)
Get the desired Euler angles for optitrack stabilization.
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...
General stabilization interface for rotorcrafts.
static void reset_vertical_vars(void)
Reset all vertical variables:
#define OFH_PGAINX
float d_err
difference of error for the D-gain
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
#define OFH_HOVER_METHOD
bool oscphi
struct Int32Vect2 of_hover_ref_pos
#define OFH_IGAINY
int16_t flowY
struct Electrical electrical
Definition: electrical.c:66
struct OFhistory historyX
#define OF_LP_CONST
#define OFH_RAMPXY
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
#define MAX_SPEED_ERR
static void reset_horizontal_vars(void)
Reset all horizontal variables:
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
float D
D-gain for control.
#define OFH_IGAINZ
void vertical_ctrl_module_init(void)
Initialize the vertical optical flow hover module.
void horizontal_ctrl_module_run(bool in_flight)
Run the horizontal optical flow hover module.
float divergence
Divergence estimate.
static abi_event optical_flow_ev
void guidance_v_module_enter(void)
Entering the vertical module (user switched to module)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
uint8_t cov_array_filledXY
static void send_optical_flow_hover(struct transport_tx *trans, struct link_device *dev)
#define PPRZ_ITRIG_COS(_c, _a)
#define OFH_OSCTHETA
struct OFhistory historyY
float PID_flow_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
Determine and set the desired angle for constant flow control.