#include "optical_flow_hover.h"
#include "optical_flow_functions.h"
#include "generated/airframe.h"
#include "autopilot.h"
#include "paparazzi.h"
#include "modules/core/abi.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "modules/energy/electrical.h"
#include <stdio.h>
#include "modules/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"
#include "math/pprz_stat.h"

Include dependency graph for optical_flow_hover.c:

Macros
#define	OFH_OPTICAL_FLOW_ID ABI_BROADCAST

#define	OFH_HOVER_METHOD 1

#define	XY_SYMMETRICAL 0

#define	OFH_COV_METHOD 0

#define	COV_WINDOW_SIZE (10*30)

#define	OF_COV_DELAY_STEPS COV_WINDOW_SIZE/2

#define	OF_LP_CONST 0.5

#define	OFH_OSCPHI 1

#define	OFH_OSCTHETA 1

#define	OFH_PGAINZ 0.4

#define	OFH_IGAINZ 0.f

#define	OFH_DGAINZ 0.0

#define	OFH_RAMPZ 0.15

#define	OFH_REDUCTIONZ 0.45

#define	OFH_COVDIV_SETPOINT -0.02

#define	OFH_PGAINX 0.f

#define	OFH_IGAINX 0.00002

#define	OFH_DGAINX 0.f

#define	OFH_PGAINY 0.f

#define	OFH_IGAINY 0.00002

#define	OFH_DGAINY 0.f

#define	OFH_RAMPXY 0.0008

#define	OFH_REDUCTIONXY 0.3

#define	OFH_COVFLOW_SETPOINT -500.f

#define	OFH_VER_SLOPE_A 0.5

#define	OFH_VER_SLOPE_B 0.25

#define	OFH_HOR_X_SLOPE_A 2.f

#define	OFH_HOR_X_SLOPE_B 0.5

#define	OFH_HOR_Y_SLOPE_A OFH_HOR_X_SLOPE_A

#define	OFH_HOR_Y_SLOPE_B OFH_HOR_X_SLOPE_B

#define	GH_GAIN_SCALE 2

#define	MAX_POS_ERR POS_BFP_OF_REAL(16.)

#define	MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)

Functions
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. More...

static void	reset_horizontal_vars (void)
	Reset all horizontal variables: More...

static void	reset_vertical_vars (void)
	Reset all vertical variables: More...

void	vertical_ctrl_module_run (void)
	Run the vertical optical flow hover module. More...

void	horizontal_ctrl_module_run (void)
	Run the horizontal optical flow hover module. More...

void	computeOptiTrack (bool phi, bool theta, struct Int32Eulers *opti_sp_eu)
	Get the desired Euler angles for optitrack stabilization. More...

static void	send_optical_flow_hover (struct transport_tx trans, struct link_device dev)

void	optical_flow_hover_init ()

void	guidance_module_enter (void)
	Entering the module (user switched to module) More...

void	guidance_module_run (bool in_flight)

Variables
struct DesiredInputs	des_inputs

struct FloatVect3	covariances

float	vision_time

float	prev_vision_timeXY

float	prev_vision_timeZ

bool	oscillatingX

bool	oscillatingY

int32_t	flowX

int32_t	flowY

struct OFhistory	historyX

struct OFhistory	historyY

struct Int32Eulers	ofh_sp_eu

bool	oscillatingZ

float	divergence_vision

struct OFhistory	historyZ

static abi_event	optical_flow_ev

struct OpticalFlowHoverControl	of_hover_ctrl_X

struct OpticalFlowHoverControl	of_hover_ctrl_Y

struct OpticalFlowHoverControl	of_hover_ctrl_Z

bool	oscphi

bool	osctheta

bool	derotated

bool	cov_method
	method to calculate the covariance: between thrust and div / angle and flow (0) or div and div past / flow and past flow(1) More...

uint8_t	hover_method
	Method used to hover 0 = All axis after each other; 1 = all axis at the same time; 2 = vertical only, use relation to set horizontal. More...

struct Int32Vect2	of_hover_pos_err

struct Int32Vect2	of_hover_speed_err

struct Int32Vect2	of_hover_ref_pos

struct Int32Vect2	of_hover_trim_att_integrator

struct Int32Vect2	of_hover_cmd_earth

Macro Definition Documentation

◆ COV_WINDOW_SIZE

#define COV_WINDOW_SIZE (10*30)

Definition at line 65 of file optical_flow_hover.c.

◆ GH_GAIN_SCALE

#define GH_GAIN_SCALE 2

Definition at line 233 of file optical_flow_hover.c.

◆ MAX_POS_ERR

#define MAX_POS_ERR POS_BFP_OF_REAL(16.)

Definition at line 236 of file optical_flow_hover.c.

◆ MAX_SPEED_ERR

#define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)

Definition at line 239 of file optical_flow_hover.c.

◆ OF_COV_DELAY_STEPS

#define OF_COV_DELAY_STEPS COV_WINDOW_SIZE/2

Definition at line 70 of file optical_flow_hover.c.

◆ OF_LP_CONST

#define OF_LP_CONST 0.5

Definition at line 75 of file optical_flow_hover.c.

◆ OFH_COV_METHOD

#define OFH_COV_METHOD 0

Definition at line 60 of file optical_flow_hover.c.

◆ OFH_COVDIV_SETPOINT

#define OFH_COVDIV_SETPOINT -0.02

Definition at line 113 of file optical_flow_hover.c.

◆ OFH_COVFLOW_SETPOINT

#define OFH_COVFLOW_SETPOINT -500.f

Definition at line 153 of file optical_flow_hover.c.

◆ OFH_DGAINX

#define OFH_DGAINX 0.f

Definition at line 126 of file optical_flow_hover.c.

◆ OFH_DGAINY

#define OFH_DGAINY 0.f

Definition at line 138 of file optical_flow_hover.c.

◆ OFH_DGAINZ

#define OFH_DGAINZ 0.0

Definition at line 98 of file optical_flow_hover.c.

◆ OFH_HOR_X_SLOPE_A

#define OFH_HOR_X_SLOPE_A 2.f

Definition at line 166 of file optical_flow_hover.c.

◆ OFH_HOR_X_SLOPE_B

#define OFH_HOR_X_SLOPE_B 0.5

Definition at line 170 of file optical_flow_hover.c.

◆ OFH_HOR_Y_SLOPE_A

#define OFH_HOR_Y_SLOPE_A OFH_HOR_X_SLOPE_A

Definition at line 174 of file optical_flow_hover.c.

◆ OFH_HOR_Y_SLOPE_B

#define OFH_HOR_Y_SLOPE_B OFH_HOR_X_SLOPE_B

Definition at line 178 of file optical_flow_hover.c.

◆ OFH_HOVER_METHOD

#define OFH_HOVER_METHOD 1

Definition at line 50 of file optical_flow_hover.c.

◆ OFH_IGAINX

#define OFH_IGAINX 0.00002

Definition at line 122 of file optical_flow_hover.c.

◆ OFH_IGAINY

#define OFH_IGAINY 0.00002

Definition at line 134 of file optical_flow_hover.c.

◆ OFH_IGAINZ

#define OFH_IGAINZ 0.f

Definition at line 94 of file optical_flow_hover.c.

◆ OFH_OPTICAL_FLOW_ID

#define OFH_OPTICAL_FLOW_ID ABI_BROADCAST

Definition at line 44 of file optical_flow_hover.c.

◆ OFH_OSCPHI

#define OFH_OSCPHI 1

Definition at line 80 of file optical_flow_hover.c.

◆ OFH_OSCTHETA

#define OFH_OSCTHETA 1

Definition at line 85 of file optical_flow_hover.c.

◆ OFH_PGAINX

#define OFH_PGAINX 0.f

Definition at line 118 of file optical_flow_hover.c.

◆ OFH_PGAINY

#define OFH_PGAINY 0.f

Definition at line 130 of file optical_flow_hover.c.

◆ OFH_PGAINZ

#define OFH_PGAINZ 0.4

Definition at line 90 of file optical_flow_hover.c.

◆ OFH_RAMPXY

#define OFH_RAMPXY 0.0008

Definition at line 143 of file optical_flow_hover.c.

◆ OFH_RAMPZ

#define OFH_RAMPZ 0.15

Definition at line 103 of file optical_flow_hover.c.

◆ OFH_REDUCTIONXY

#define OFH_REDUCTIONXY 0.3

Definition at line 148 of file optical_flow_hover.c.

◆ OFH_REDUCTIONZ

#define OFH_REDUCTIONZ 0.45

Definition at line 108 of file optical_flow_hover.c.

◆ OFH_VER_SLOPE_A

#define OFH_VER_SLOPE_A 0.5

Definition at line 158 of file optical_flow_hover.c.

◆ OFH_VER_SLOPE_B

#define OFH_VER_SLOPE_B 0.25

Definition at line 162 of file optical_flow_hover.c.

◆ XY_SYMMETRICAL

#define XY_SYMMETRICAL 0

Definition at line 55 of file optical_flow_hover.c.

Function Documentation

◆ computeOptiTrack()

void computeOptiTrack	(	bool	phi,
		bool	theta,
		struct Int32Eulers *	opti_sp_eu
	)

Get the desired Euler angles for optitrack stabilization.

Parameters

[in]	Boolean	whether to Phi or not
[in]	Boolean	whether to Theta or not
[out]	The	desired Euler angles

Definition at line 636 of file optical_flow_hover.c.

References BFP_OF_REAL, GH_GAIN_SCALE, GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC, INT32_POS_FRAC, INT32_SPEED_FRAC, INT32_TRIG_FRAC, MAX_POS_ERR, MAX_SPEED_ERR, Min, of_hover_cmd_earth, of_hover_pos_err, of_hover_ref_pos, of_hover_speed_err, of_hover_trim_att_integrator, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, stateGetNedToBodyEulers_i(), stateGetPositionNed_i(), stateGetSpeedNed_i(), Int32Eulers::theta, VECT2_DIFF, VECT2_STRIM, Int32Vect2::x, and Int32Vect2::y.

Referenced by horizontal_ctrl_module_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ guidance_module_enter()

void guidance_module_enter ( void )

Entering the module (user switched to module)

Definition at line 600 of file optical_flow_hover.c.

References Stabilization::cmd, des_inputs, MAX_PPRZ, OpticalFlowHoverControl::nominal_value, of_hover_ctrl_Z, of_hover_ref_pos, ofh_sp_eu, Int32Eulers::psi, reset_horizontal_vars(), reset_vertical_vars(), stabilization, stateGetNedToBodyEulers_i(), stateGetPositionNed_i(), DesiredInputs::thrust, and VECT2_COPY.

Here is the call graph for this function:

◆ guidance_module_run()

void guidance_module_run ( bool in_flight )

Definition at line 616 of file optical_flow_hover.c.

References AP_MODE_NAV, autopilot_set_mode(), Electrical::bat_low, Stabilization::cmd, des_inputs, electrical, horizontal_ctrl_module_run(), ofh_sp_eu, StabilizationSetpoint::sp, ThrustSetpoint::sp, stab_sp_from_eulers_i(), stabilization, stabilization_attitude_run(), th_sp_from_thrust_i(), DesiredInputs::thrust, THRUST_AXIS_Z, and vertical_ctrl_module_run().

Here is the call graph for this function:

◆ horizontal_ctrl_module_run()

void horizontal_ctrl_module_run ( void )

Run the horizontal optical flow hover module.

Definition at line 409 of file optical_flow_hover.c.

References BFP_OF_REAL, computeOptiTrack(), cov_method, OpticalFlowHoverControl::cov_setpoint, covariances, des_inputs, GainsPID::err, OpticalFlowHover::flowX, flowX, OpticalFlowHover::flowY, flowY, historyX, historyY, hover_method, INT32_ANGLE_FRAC, lp_factor, OpticalFlowHoverControl::nominal_value, of_hover, of_hover_ctrl_X, of_hover_ctrl_Y, OF_LP_CONST, OFH_PGAINY, ofh_sp_eu, oscillatingX, oscillatingY, oscphi, osctheta, GainsPID::P, Int32Eulers::phi, DesiredInputs::phi, OpticalFlowHoverControl::PID, PID_flow_control(), prev_vision_timeXY, OpticalFlowHoverControl::ramp, OpticalFlowHoverControl::reduction_factor, set_cov_flow(), OpticalFlowHoverControl::setpoint, Int32Eulers::theta, DesiredInputs::theta, vision_time, FloatVect3::x, XY_SYMMETRICAL, and FloatVect3::y.

Referenced by guidance_module_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ofh_optical_flow_cb()

void ofh_optical_flow_cb	(	uint8_t sender_id	UNUSED,
		uint32_t	stamp,
		int32_t	flow_x,
		int32_t	flow_y,
		int32_t	flow_der_x,
		int32_t	flow_der_y,
		float quality	UNUSED,
		float	size_div
	)