Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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"
23
24#include "generated/airframe.h"
25#include "autopilot.h"
26#include "paparazzi.h"
27#include "modules/core/abi.h"
32#include <stdio.h>
33
35//
37#include "mcu_periph/sys_time.h"
38//
39// Additional math functions
40#include "math/pprz_stat.h"
41
42/* Use optical flow estimates */
43#ifndef OFH_OPTICAL_FLOW_ID
44#define OFH_OPTICAL_FLOW_ID ABI_BROADCAST
45#endif
47
48// Set the standard method (1) to apply the algorithm to all 3 axes at the same time
49#ifndef OFH_HOVER_METHOD
50#define OFH_HOVER_METHOD 1
51#endif
52
53// Set the standard method to assume the quadrotor is not (0) symmetrical
54#ifndef XY_SYMMETRICAL
55#define XY_SYMMETRICAL 0
56#endif
57
58// Set the standard method to use vision signal and the known input and signal (0) optionally one can use the autocovariance (1)
59#ifndef OFH_COV_METHOD
60#define OFH_COV_METHOD 0
61#endif
62
63// number of time steps used for calculating the covariance (oscillations) and the delay steps
64#ifndef COV_WINDOW_SIZE
65#define COV_WINDOW_SIZE (10*30)
66#endif
67
68// In case the autocovariance is used, select half the window size as the delay
69#ifndef OF_COV_DELAY_STEPS
70#define OF_COV_DELAY_STEPS COV_WINDOW_SIZE/2
71#endif
72
73// The low pass filter constant for updating the vision measurements in the algorithm
74#ifndef OF_LP_CONST
75#define OF_LP_CONST 0.5
76#endif
77
78// Whether the algorithm should be applied to the phi angle(1) or not(0)
79#ifndef OFH_OSCPHI
80#define OFH_OSCPHI 1
81#endif
82
83// Whether the algorithm should be applied to the theta angle(1) or not(0)
84#ifndef OFH_OSCTHETA
85#define OFH_OSCTHETA 1
86#endif
87
88// Use default PID gains
89#ifndef OFH_PGAINZ
90#define OFH_PGAINZ 0.4
91#endif
92
93#ifndef OFH_IGAINZ
94#define OFH_IGAINZ 0.f
95#endif
96
97#ifndef OFH_DGAINZ
98#define OFH_DGAINZ 0.0
99#endif
100
101// Default slope at which the gain is increased
102#ifndef OFH_RAMPZ
103#define OFH_RAMPZ 0.15
104#endif
105
106// Default reduction factor to stabilize the quad after oscillating
107#ifndef OFH_REDUCTIONZ
108#define OFH_REDUCTIONZ 0.45
109#endif
110
111// The threshold value for the covariance oscillation measurement
112#ifndef OFH_COVDIV_SETPOINT
113#define OFH_COVDIV_SETPOINT -0.02
114#endif
115
116// Use default PID gains
117#ifndef OFH_PGAINX
118#define OFH_PGAINX 0.f
119#endif
120
121#ifndef OFH_IGAINX
122#define OFH_IGAINX 0.00002
123#endif
124
125#ifndef OFH_DGAINX
126#define OFH_DGAINX 0.f
127#endif
128
129#ifndef OFH_PGAINY
130#define OFH_PGAINY 0.f
131#endif
132
133#ifndef OFH_IGAINY
134#define OFH_IGAINY 0.00002
135#endif
136
137#ifndef OFH_DGAINY
138#define OFH_DGAINY 0.f
139#endif
140
141// Default slope at which the gain is increased
142#ifndef OFH_RAMPXY
143#define OFH_RAMPXY 0.0008
144#endif
145
146// Default reduction factor to stabilize the quad after oscillating
147#ifndef OFH_REDUCTIONXY
148#define OFH_REDUCTIONXY 0.3
149#endif
150
151// The threshold value for the covariance oscillation measurement
152#ifndef OFH_COVFLOW_SETPOINT
153#define OFH_COVFLOW_SETPOINT -500.f
154#endif
155
156// The default slopes for the height-gain relationships
157#ifndef OFH_VER_SLOPE_A
158#define OFH_VER_SLOPE_A 0.5
159#endif
160
161#ifndef OFH_VER_SLOPE_B
162#define OFH_VER_SLOPE_B 0.25
163#endif
164
165#ifndef OFH_HOR_X_SLOPE_A
166#define OFH_HOR_X_SLOPE_A 2.f
167#endif
168
169#ifndef OFH_HOR_X_SLOPE_B
170#define OFH_HOR_X_SLOPE_B 0.5
171#endif
172
173#ifndef OFH_HOR_Y_SLOPE_A
174#define OFH_HOR_Y_SLOPE_A OFH_HOR_X_SLOPE_A
175#endif
176
177#ifndef OFH_HOR_Y_SLOPE_B
178#define OFH_HOR_Y_SLOPE_B OFH_HOR_X_SLOPE_B
179#endif
180
181
184
185// variables retained between module calls
187
194
195// Stabilizing commands
197
200
202
203// The optical flow ABI event
205
206// struct containing most relevant parameters
210
211// variables used in the GCS
217
218
220// Callback function of the optical flow estimate:
221void ofh_optical_flow_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp, int32_t flow_x, int32_t flow_y,
222 int32_t flow_der_x, int32_t flow_der_y, float quality, float size_div);
223
224// resetting all variables to be called for instance when starting up / re-entering module
225static void reset_horizontal_vars(void);
226static void reset_vertical_vars(void);
227void vertical_ctrl_module_run(void);
229
230// Compute OptiTrack stabilization for 1/2 axes
231void 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:
249static 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,
253 &of_hover_ctrl_X.PID.sum_err, &of_hover_ctrl_Y.PID.sum_err, &of_hover_ctrl_Z.PID.sum_err,
254 &des_inputs.thrust, &des_inputs.phi, &des_inputs.theta);
255}
256
257
258// Init the optical flow hover module
260{
261 of_hover_ctrl_X.setpoint = 0.0f;
267 of_hover_ctrl_X.reduction_factor = OFH_REDUCTIONXY;
268
269 of_hover_ctrl_Y.setpoint = 0.0f;
275 of_hover_ctrl_Y.reduction_factor = OFH_REDUCTIONXY;
276
277 of_hover_ctrl_Z.setpoint = 0.0f;
283 of_hover_ctrl_Z.reduction_factor = OFH_REDUCTIONZ;
284
287
290
293
294 // Subscribe to the optical flow estimator:
296
297 // register telemetry:
299}
300
304static void reset_horizontal_vars(void)
305{
306 // Take the current angles as neutral
307 struct Int32Eulers tempangle = stab_sp_to_eulers_i(&stabilization.sp);
308 of_hover_ctrl_X.nominal_value = DegOfRad(FLOAT_OF_BFP(tempangle.phi, INT32_ANGLE_FRAC));
309 of_hover_ctrl_Y.nominal_value = DegOfRad(FLOAT_OF_BFP(tempangle.theta, INT32_ANGLE_FRAC));
310
311 des_inputs.phi = 0;
312 des_inputs.theta = 0;
313
314 // FIXME module is always used for vertical control
315 if ((hover_method == 0)) {
316 // Z - X - Y Order
317 oscillatingX = 1;
318 oscillatingY = 1;
321 } else if ((hover_method == 2)) {
322 // Z Set XY
323 oscillatingX = 1;
324 oscillatingY = 1;
327 of_hover_ctrl_X.PID.I = OFH_IGAINX / 4; // Have a slighly lower I gain during Z
328 of_hover_ctrl_Y.PID.I = OFH_IGAINY / 4; // Have a slighly lower I gain during Z
329 } else {
330 // if V is in NAV or hover_method = 1 FIXME NAV is not called for vertical
331 //All axes
332 oscillatingX = 0;
333 oscillatingY = 0;
336 }
337
338 flowX = 0;
339 flowY = 0;
340
341 of_hover_ctrl_X.PID.sum_err = 0.0f;
342 of_hover_ctrl_X.PID.d_err = 0.0f;
343 of_hover_ctrl_X.PID.previous_err = 0.0f;
344
345 of_hover_ctrl_Y.PID.sum_err = 0.0f;
346 of_hover_ctrl_Y.PID.d_err = 0.0f;
347 of_hover_ctrl_Y.PID.previous_err = 0.0f;
348
349 ofh_sp_eu.phi = of_hover_ctrl_X.nominal_value;
350 ofh_sp_eu.theta = of_hover_ctrl_Y.nominal_value;
351
352 ind_histXY = 0;
354
355 covariances.x = 0.0f;
356 covariances.y = 0.0f;
357
358 for (uint16_t i = 0; i < COV_WINDOW_SIZE; i++) {
359 historyX.OF[i] = 0.0f;
360 historyY.OF[i] = 0.0f;
361 historyX.past_OF[i] = 0.0f;
362 historyY.past_OF[i] = 0.0f;
363 historyX.input[i] = 0.0f;
364 historyY.input[i] = 0.0f;
365 }
366
367 of_hover.flowX = 0;
368 of_hover.flowY = 0;
369
372}
373
377static void reset_vertical_vars(void)
378{
379 oscillatingZ = 0;
380
382 of_hover.divergence = 0;
383
384 for (uint16_t i = 0; i < COV_WINDOW_SIZE; i++) {
385 historyZ.OF[i] = 0.0f;
386 historyZ.past_OF[i] = 0.0f;
387 historyZ.input[i] = 0.0f;
388 }
389
390 ind_histZ = 0;
391
392 covariances.z = 0.0f;
394
396
397 of_hover_ctrl_Z.PID.sum_err = 0.0f;
398 of_hover_ctrl_Z.PID.d_err = 0.0f;
399 of_hover_ctrl_Z.PID.previous_err = 0.0f;
400
403}
404
405
410{
411 /***********
412 * TIME
413 ***********/
414 float ventral_factor = -1.28f; // magic number comprising field of view etc.
415
416 float dt = vision_time - prev_vision_timeXY;
417
418 // check if new measurement received
419 if (dt <= 1e-5f) {
420 return;
421 }
422
423 /***********
424 * VISION
425 ***********/
426
427 float lp_factor = dt / OF_LP_CONST;
428 Bound(lp_factor, 0.f, 1.f);
429
430 float new_flowX = (flowX * ventral_factor) / dt;
431 float new_flowY = (flowY * ventral_factor) / dt;
432
433 //TODO: deal with (unlikely) fast changes in Flow?
434
435 // low-pass filter the divergence:
436 of_hover.flowX += (new_flowX - of_hover.flowX) * lp_factor;
437 of_hover.flowY += (new_flowY - of_hover.flowY) * lp_factor;
438
439
440 /***********
441 * CONTROL
442 ***********/
443
444 if (!oscillatingX) {
445 // if not oscillating, increase gain
446 of_hover_ctrl_X.PID.P += of_hover_ctrl_X.ramp * dt;
447 }
448 if (!oscillatingY) {
449 // if not oscillating, increase gain
450 of_hover_ctrl_Y.PID.P += of_hover_ctrl_Y.ramp * dt;
451 }
452
453 // set desired pitch en roll
454 if (oscphi) {
455 of_hover_ctrl_X.PID.err = of_hover_ctrl_X.setpoint - of_hover.flowX;
457 }
458 if (osctheta) {
459 of_hover_ctrl_Y.PID.err = of_hover_ctrl_Y.setpoint - of_hover.flowY;
460 des_inputs.theta = of_hover_ctrl_Y.nominal_value + PID_flow_control(dt, &of_hover_ctrl_Y);
461 }
462
463 // update covariance
466 ofh_sp_eu.theta = BFP_OF_REAL(RadOfDeg(des_inputs.theta * osctheta), INT32_ANGLE_FRAC);
467
468 // Check for oscillations
469 if ((covariances.x < of_hover_ctrl_X.cov_setpoint) && (!oscillatingX)) {
470 oscillatingX = 1;
471
472 if (hover_method == 0) {
473 //Start the Y axis
474 oscillatingY = 0;
476 }
477 of_hover_ctrl_X.PID.P = of_hover_ctrl_X.PID.P * of_hover_ctrl_X.reduction_factor;
478
479 if (XY_SYMMETRICAL) {
480 // also set Y
481 oscillatingY = 1;
482 of_hover_ctrl_Y.PID.P = of_hover_ctrl_X.PID.P;
483 }
484 }
485 if ((covariances.y < of_hover_ctrl_Y.cov_setpoint) && (!oscillatingY)) {
486 oscillatingY = 1;
487 of_hover_ctrl_Y.PID.P = of_hover_ctrl_Y.PID.P * of_hover_ctrl_Y.reduction_factor;
488 }
489
490 // Compute 0, 1 or 2 horizontal axes with optitrack
492
494}
495
500{
501 /***********
502 * TIME
503 ***********/
504
505 float div_factor; // factor that maps divergence in pixels as received from vision to 1 / frame
506
507 float dt = vision_time - prev_vision_timeZ;
508
509 // check if new measurement received
510 if (dt <= 1e-5f) {
511 return;
512 }
513
514 /***********
515 * VISION
516 ***********/
517
518 float lp_factor = dt / OF_LP_CONST;
519 Bound(lp_factor, 0.f, 1.f);
520
521 // Vision
522 div_factor = -1.28f; // magic number comprising field of view etc.
523 float new_divergence = (divergence_vision * div_factor) / dt;
524
525 // deal with (unlikely) fast changes in divergence:
526 static const float max_div_dt = 0.20f;
527 if (fabsf(new_divergence - of_hover.divergence) > max_div_dt) {
528 if (new_divergence < of_hover.divergence) { new_divergence = of_hover.divergence - max_div_dt; }
529 else { new_divergence = of_hover.divergence + max_div_dt; }
530 }
531 // low-pass filter the divergence:
532
533 of_hover.divergence += (new_divergence - of_hover.divergence) * lp_factor;
535
536 /***********
537 * CONTROL
538 ***********/
539 if (!oscillatingZ) {
540 // if not oscillating, increase gain
541 of_hover_ctrl_Z.PID.P += of_hover_ctrl_Z.ramp * dt;
542 }
543
544 // use the divergence for control:
545 of_hover_ctrl_Z.PID.err = of_hover_ctrl_Z.setpoint - of_hover.divergence;
547
549
550 // Check for oscillations
551 if (covariances.z < of_hover_ctrl_Z.cov_setpoint && (!oscillatingZ)) {
552 float estimatedHeight = of_hover_ctrl_Z.PID.P * OFH_VER_SLOPE_A + OFH_VER_SLOPE_B; // Vertical slope Z
553 oscillatingZ = 1;
554 of_hover_ctrl_Z.setpoint = 0.0f;
555 of_hover_ctrl_Z.PID.P = of_hover_ctrl_Z.PID.P * of_hover_ctrl_Z.reduction_factor;
556
557 if (hover_method == 0) {
558 // Z- X - Y Order
559 // Start the X axis
560 oscillatingX = 0;
562 } else if (hover_method == 2) {
563 // Start XY axes with computed slope
564 of_hover_ctrl_X.PID.sum_err = 0.0f;
565 of_hover_ctrl_Y.PID.sum_err = 0.0f;
568 of_hover_ctrl_X.PID.P = OFH_REDUCTIONXY * (estimatedHeight * OFH_HOR_X_SLOPE_A - OFH_HOR_X_SLOPE_B); // Horizontal Slope X
569 of_hover_ctrl_Y.PID.P = OFH_REDUCTIONXY * (estimatedHeight * OFH_HOR_Y_SLOPE_A - OFH_HOR_Y_SLOPE_B); // Horizontal Slope Y
570 } else {
571 // hover_method = 1
572 //All axes
573 }
574 }
575
576}
577
578void ofh_optical_flow_cb(uint8_t sender_id UNUSED, uint32_t stamp, int32_t flow_x, int32_t flow_y,
579 int32_t flow_der_x, int32_t flow_der_y, float quality UNUSED, float size_div)
580{
581 if (!derotated) {
582 flowX = flow_x;
583 flowY = flow_y;
584 } else {
585 flowX = flow_der_x;
586 flowY = flow_der_y;
587 }
588
589 divergence_vision = size_div;
590
591 vision_time = ((float)stamp) / 1e6;
592}
593
595// Call our controller
596
601{
603
604 // adaptive estimation - assume hover condition when entering the module
605 of_hover_ctrl_Z.nominal_value = (float) stabilization.cmd[COMMAND_THRUST] / MAX_PPRZ;
606 des_inputs.thrust = (int32_t) of_hover_ctrl_Z.nominal_value * MAX_PPRZ;
607
608 // Set current psi as heading
610
613}
614
615// Run the controller
616void guidance_module_run(bool in_flight)
617{
618 if (electrical.bat_low) {
620 } else {
621 // your controller goes here
626 stabilization_attitude_run(in_flight, &sp, &th, stabilization.cmd);
627 }
628}
629
636void computeOptiTrack(bool phi, bool theta, struct Int32Eulers *opti_sp_eu)
637{
638
639 bool optiVelOnly;
640 optiVelOnly = 0;
641
643
644 struct NedCoor_i vel_from_GPS;
645 struct NedCoor_i pos_from_GPS;
646
647 vel_from_GPS = *stateGetSpeedNed_i();
648 pos_from_GPS = *stateGetPositionNed_i();
649
650 /* maximum bank angle: default 20 deg, max 40 deg*/
651 static const int32_t traj_max_bank = Min(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC),
652 BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC));
653 static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC);
654
655 /* compute position error */
657 /* saturate it */
659
660 struct Int32Vect2 ref_speed;
661 ref_speed.x = 0;
662 ref_speed.y = 0;
663
664 /* compute speed error */
665 VECT2_DIFF(of_hover_speed_err, ref_speed, vel_from_GPS);
666 /* saturate it */
668
669 if (optiVelOnly) {
670 of_hover_pos_err.x = 0;
671 of_hover_pos_err.y = 0;
672 }
673
674 /* run PID */
676 ((GUIDANCE_H_PGAIN * of_hover_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
677 ((GUIDANCE_H_DGAIN * (of_hover_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
679 ((GUIDANCE_H_PGAIN * of_hover_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
680 ((GUIDANCE_H_DGAIN * (of_hover_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
681
682 /* trim max bank angle from PD */
683 VECT2_STRIM(of_hover_cmd_earth, -traj_max_bank, traj_max_bank);
684
685 of_hover_trim_att_integrator.x += (GUIDANCE_H_IGAIN * of_hover_cmd_earth.x);
686 of_hover_trim_att_integrator.y += (GUIDANCE_H_IGAIN * of_hover_cmd_earth.y);
687 /* saturate it */
689 (traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2)));
690
691 /* add it to the command */
694
695 VECT2_STRIM(of_hover_cmd_earth, -total_max_bank, total_max_bank);
696
697 // Compute Angle Setpoints - Taken from Stab_att_quat
698 int32_t s_psi, c_psi;
699 PPRZ_ITRIG_SIN(s_psi, psi);
700 PPRZ_ITRIG_COS(c_psi, psi);
701
702 if (phi) {
703 opti_sp_eu->phi = (-s_psi * of_hover_cmd_earth.x + c_psi * of_hover_cmd_earth.y) >> INT32_TRIG_FRAC;
704 }
705 if (theta) {
706 opti_sp_eu->theta = -(c_psi * of_hover_cmd_earth.x + s_psi * of_hover_cmd_earth.y) >> INT32_TRIG_FRAC;
707 }
708}
709
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition autopilot.c:200
Core autopilot interface common to all firmwares.
#define UNUSED(x)
static struct uart_periph * dev
struct Electrical electrical
Definition electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
#define Min(x, y)
Definition esc_dshot.c:110
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_STRIM(_v, _min, _max)
#define VECT2_COPY(_a, _b)
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
#define INT32_POS_FRAC
#define INT32_TRIG_FRAC
#define INT32_SPEED_FRAC
#define FLOAT_OF_BFP(_vbfp, _frac)
#define INT32_ANGLE_FRAC
#define BFP_OF_REAL(_vr, _frac)
euler angles
vector in North East Down coordinates
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition state.h:1307
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition state.h:805
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition state.h:1015
#define MAX_SPEED_ERR
#define MAX_POS_ERR
#define GH_GAIN_SCALE
float lp_factor
Definition ins_flow.c:182
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
uint8_t cov_array_filledXY
int32_t PID_divergence_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
Determine and set the thrust for constant divergence control.
struct OpticalFlowHover of_hover
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...
float PID_flow_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
Determine and set the desired angle for constant flow control.
uint32_t ind_histXY
uint32_t ind_histZ
uint8_t cov_array_filledZ
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...
#define OF_LP_CONST
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.
static void reset_horizontal_vars(void)
Reset all horizontal variables:
void computeOptiTrack(bool phi, bool theta, struct Int32Eulers *opti_sp_eu)
Get the desired Euler angles for optitrack stabilization.
static abi_event optical_flow_ev
static void send_optical_flow_hover(struct transport_tx *trans, struct link_device *dev)
float prev_vision_timeZ
void horizontal_ctrl_module_run(void)
Run the horizontal optical flow hover module.
bool oscillatingX
bool derotated
struct OpticalFlowHoverControl of_hover_ctrl_Y
#define OFH_REDUCTIONZ
struct FloatVect3 covariances
struct Int32Vect2 of_hover_pos_err
int32_t flowX
struct Int32Vect2 of_hover_cmd_earth
#define OFH_DGAINZ
int32_t flowY
#define XY_SYMMETRICAL
struct DesiredInputs des_inputs
uint8_t hover_method
Method used to hover 0 = All axis after each other; 1 = all axis at the same time; 2 = vertical only,...
#define COV_WINDOW_SIZE
#define OFH_OSCTHETA
bool oscphi
#define OFH_IGAINY
#define OFH_IGAINZ
#define OFH_PGAINY
#define OFH_COV_METHOD
struct OpticalFlowHoverControl of_hover_ctrl_Z
#define OFH_HOR_X_SLOPE_B
#define OFH_HOR_X_SLOPE_A
struct OFhistory historyZ
float prev_vision_timeXY
#define OFH_OPTICAL_FLOW_ID
float divergence_vision
#define OFH_HOVER_METHOD
#define OFH_PGAINX
void guidance_module_enter(void)
Entering the module (user switched to module).
void guidance_module_run(bool in_flight)
#define OFH_RAMPZ
struct OFhistory historyX
#define OFH_COVFLOW_SETPOINT
static void reset_vertical_vars(void)
Reset all vertical variables:
bool oscillatingZ
#define OFH_REDUCTIONXY
void optical_flow_hover_init()
#define OFH_PGAINZ
#define OFH_DGAINX
#define OFH_VER_SLOPE_B
float vision_time
struct Int32Vect2 of_hover_ref_pos
#define OFH_DGAINY
struct OFhistory historyY
#define OFH_IGAINX
bool cov_method
method to calculate the covariance: between thrust and div / angle and flow (0) or div and div past /...
#define OFH_COVDIV_SETPOINT
struct Int32Vect2 of_hover_speed_err
struct OpticalFlowHoverControl of_hover_ctrl_X
struct Int32Eulers ofh_sp_eu
bool oscillatingY
void vertical_ctrl_module_run(void)
Run the vertical optical flow hover module.
#define OFH_RAMPXY
bool osctheta
#define OFH_VER_SLOPE_A
struct Int32Vect2 of_hover_trim_att_integrator
#define OFH_OSCPHI
#define OFH_HOR_Y_SLOPE_B
#define OFH_HOR_Y_SLOPE_A
#define MAX_PPRZ
Definition paparazzi.h:8
Statistics functions.
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
#define AP_MODE_NAV
#define GUIDANCE_H_MAX_BANK
Max bank controlled by guidance.
Definition guidance_h.h:64
Vertical guidance for rotorcrafts.
General attitude stabilization interface for rotorcrafts.
struct Stabilization stabilization
struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers)
struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
General stabilization interface for rotorcrafts.
#define THRUST_AXIS_Z
void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Attitude control run function.
Stabilization setpoint.
union StabilizationSetpoint::@354356152067340015216250042204214023005305374154 sp
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
union ThrustSetpoint::@302344065342070214103253314124156277107357236133 sp
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:241
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.