#include "optical_flow_hover.h"
#include "optical_flow_functions.h"
#include "generated/airframe.h"
#include "paparazzi.h"
#include "subsystems/abi.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "subsystems/electrical.h"
#include <stdio.h>
#include "subsystems/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, 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. 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_init (void)
	Initialize the vertical optical flow hover module. More...

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

void	horizontal_ctrl_module_init (void)
	Initialize the horizontal optical flow hover module. More...

void	horizontal_ctrl_module_run (bool in_flight)
	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_h_module_read_rc (void)

void	guidance_v_module_init (void)

void	guidance_h_module_init (void)

void	guidance_v_module_enter (void)
	Entering the vertical module (user switched to module) More...

void	guidance_h_module_enter (void)
	Entering the horizontal module (user switched to module) More...

void	guidance_v_module_run (bool in_flight)

void	guidance_h_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

int16_t	flowX

int16_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

#define COV_WINDOW_SIZE (10*30)

Definition at line 63 of file optical_flow_hover.c.

Referenced by reset_horizontal_vars(), reset_vertical_vars(), set_cov_div(), and set_cov_flow().

#define GH_GAIN_SCALE 2

Definition at line 233 of file optical_flow_hover.c.

Referenced by computeOptiTrack().

#define MAX_POS_ERR POS_BFP_OF_REAL(16.)

Definition at line 236 of file optical_flow_hover.c.

Referenced by computeOptiTrack().

#define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)

Definition at line 239 of file optical_flow_hover.c.

Referenced by computeOptiTrack().

#define OF_COV_DELAY_STEPS COV_WINDOW_SIZE/2

Definition at line 68 of file optical_flow_hover.c.

#define OF_LP_CONST 0.5

Definition at line 73 of file optical_flow_hover.c.

Referenced by horizontal_ctrl_module_run(), and vertical_ctrl_module_run().

#define OFH_COV_METHOD 0

Definition at line 58 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_COVDIV_SETPOINT -0.02

Definition at line 111 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_COVFLOW_SETPOINT -500.f

Definition at line 151 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_DGAINX 0.f

Definition at line 124 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_DGAINY 0.f

Definition at line 136 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_DGAINZ 0.0

Definition at line 96 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_HOR_X_SLOPE_A 2.f

Definition at line 164 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define OFH_HOR_X_SLOPE_B 0.5

Definition at line 168 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define OFH_HOR_Y_SLOPE_A OFH_HOR_X_SLOPE_A

Definition at line 172 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define OFH_HOR_Y_SLOPE_B OFH_HOR_X_SLOPE_B

Definition at line 176 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define OFH_HOVER_METHOD 1

Definition at line 48 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_IGAINX 0.00002

Definition at line 120 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init(), reset_horizontal_vars(), and vertical_ctrl_module_run().

#define OFH_IGAINY 0.00002

Definition at line 132 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init(), reset_horizontal_vars(), and vertical_ctrl_module_run().

#define OFH_IGAINZ 0.f

Definition at line 92 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_OPTICAL_FLOW_ID ABI_BROADCAST

Definition at line 42 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_OSCPHI 1

Definition at line 78 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_OSCTHETA 1

Definition at line 83 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_PGAINX 0.f

Definition at line 116 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init(), reset_horizontal_vars(), and vertical_ctrl_module_run().

#define OFH_PGAINY 0.f

Definition at line 128 of file optical_flow_hover.c.

Referenced by horizontal_ctrl_module_run(), optical_flow_hover_init(), and reset_horizontal_vars().

#define OFH_PGAINZ 0.4

Definition at line 88 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init(), and reset_vertical_vars().

#define OFH_RAMPXY 0.0008

Definition at line 141 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_RAMPZ 0.15

Definition at line 101 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_REDUCTIONXY 0.3

Definition at line 146 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init(), and vertical_ctrl_module_run().

#define OFH_REDUCTIONZ 0.45

Definition at line 106 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_VER_SLOPE_A 0.5

Definition at line 156 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define OFH_VER_SLOPE_B 0.25

Definition at line 160 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define XY_SYMMETRICAL 0

Definition at line 53 of file optical_flow_hover.c.

Referenced by horizontal_ctrl_module_run().

Function Documentation

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 684 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:

void guidance_h_module_enter ( void )

Entering the horizontal module (user switched to module)

Definition at line 645 of file optical_flow_hover.c.

References of_hover_ref_pos, ofh_sp_eu, Int32Eulers::psi, reset_horizontal_vars(), stateGetNedToBodyEulers_i(), stateGetPositionNed_i(), and VECT2_COPY.

Here is the call graph for this function:

void guidance_h_module_init ( void )

Definition at line 625 of file optical_flow_hover.c.

References horizontal_ctrl_module_init().

Here is the call graph for this function:

void guidance_h_module_read_rc ( void )

Definition at line 424 of file optical_flow_hover.c.

void guidance_h_module_run ( bool in_flight )

Definition at line 667 of file optical_flow_hover.c.

References AP_MODE_NAV, autopilot_static_set_mode(), Electrical::bat_low, electrical, horizontal_ctrl_module_run(), and stabilization_attitude_run().

Here is the call graph for this function:

void guidance_v_module_enter ( void )

Entering the vertical module (user switched to module)

Definition at line 633 of file optical_flow_hover.c.

References des_inputs, MAX_PPRZ, OpticalFlowHoverControl::nominal_value, of_hover_ctrl_Z, reset_vertical_vars(), stabilization_cmd, and DesiredInputs::thrust.

Here is the call graph for this function:

void guidance_v_module_init ( void )

Definition at line 619 of file optical_flow_hover.c.

References vertical_ctrl_module_init().

Here is the call graph for this function:

void guidance_v_module_run ( bool in_flight )

Definition at line 656 of file optical_flow_hover.c.

void horizontal_ctrl_module_init ( void )

Initialize the horizontal optical flow hover module.

Definition at line 313 of file optical_flow_hover.c.

References reset_horizontal_vars().

Referenced by guidance_h_module_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void horizontal_ctrl_module_run ( bool in_flight )

Run the horizontal optical flow hover module.

Definition at line 429 of file optical_flow_hover.c.

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

Referenced by guidance_h_module_run().

Here is the call graph for this function:

Here is the caller graph for this function:

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
	)