24 #include "generated/airframe.h"
41 #ifndef OFH_OPTICAL_FLOW_ID
42 #define OFH_OPTICAL_FLOW_ID ABI_BROADCAST
47 #ifndef OFH_HOVER_METHOD
48 #define OFH_HOVER_METHOD 1
52 #ifndef XY_SYMMETRICAL
53 #define XY_SYMMETRICAL 0
57 #ifndef OFH_COV_METHOD
58 #define OFH_COV_METHOD 0
62 #ifndef COV_WINDOW_SIZE
63 #define COV_WINDOW_SIZE (10*30)
67 #ifndef OF_COV_DELAY_STEPS
68 #define OF_COV_DELAY_STEPS COV_WINDOW_SIZE/2
73 #define OF_LP_CONST 0.5
83 #define OFH_OSCTHETA 1
88 #define OFH_PGAINZ 0.4
92 #define OFH_IGAINZ 0.f
96 #define OFH_DGAINZ 0.0
101 #define OFH_RAMPZ 0.15
105 #ifndef OFH_REDUCTIONZ
106 #define OFH_REDUCTIONZ 0.45
110 #ifndef OFH_COVDIV_SETPOINT
111 #define OFH_COVDIV_SETPOINT -0.02
116 #define OFH_PGAINX 0.f
120 #define OFH_IGAINX 0.00002
124 #define OFH_DGAINX 0.f
128 #define OFH_PGAINY 0.f
132 #define OFH_IGAINY 0.00002
136 #define OFH_DGAINY 0.f
141 #define OFH_RAMPXY 0.0008
145 #ifndef OFH_REDUCTIONXY
146 #define OFH_REDUCTIONXY 0.3
150 #ifndef OFH_COVFLOW_SETPOINT
151 #define OFH_COVFLOW_SETPOINT -500.f
155 #ifndef OFH_VER_SLOPE_A
156 #define OFH_VER_SLOPE_A 0.5
159 #ifndef OFH_VER_SLOPE_B
160 #define OFH_VER_SLOPE_B 0.25
163 #ifndef OFH_HOR_X_SLOPE_A
164 #define OFH_HOR_X_SLOPE_A 2.f
167 #ifndef OFH_HOR_X_SLOPE_B
168 #define OFH_HOR_X_SLOPE_B 0.5
171 #ifndef OFH_HOR_Y_SLOPE_A
172 #define OFH_HOR_Y_SLOPE_A OFH_HOR_X_SLOPE_A
175 #ifndef OFH_HOR_Y_SLOPE_B
176 #define OFH_HOR_Y_SLOPE_B OFH_HOR_X_SLOPE_B
220 int32_t flow_der_x,
int32_t flow_der_y,
float quality,
float size_div);
232 #ifndef GH_GAIN_SCALE
233 #define GH_GAIN_SCALE 2
236 #define MAX_POS_ERR POS_BFP_OF_REAL(16.)
238 #ifndef MAX_SPEED_ERR
239 #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
434 float ventral_factor = -1.28f;
450 float new_flowX = (
flowX * ventral_factor) / dt;
451 float new_flowY = (
flowY * ventral_factor) / dt;
548 static const float max_div_dt = 0.20f;
602 int32_t flow_der_x,
int32_t flow_der_y,
float quality,
float size_div)
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
#define GUIDANCE_V_MODE_MODULE_SETTING
struct Electrical electrical
Interface for electrical status: supply voltage, current, battery status, etc.
bool bat_low
battery low status
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
#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
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
#define FLOAT_OF_BFP(_vbfp, _frac)
#define BFP_OF_REAL(_vr, _frac)
vector in North East Down coordinates
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
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.
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.
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...
float cov_setpoint
for adaptive gain control, setpoint of the covariance (oscillations)
float d_err
difference of error for the D-gain
float P
P-gain for control.
struct OpticalFlowHover of_hover
float divergence
Divergence estimate.
struct GainsPID PID
The struct with the PID gains.
float flowX
Flow estimate in X direction.
float previous_err
Previous tracking error.
float I
I-gain for control.
float input[COV_WINDOW_SIZE]
float reduction_factor
Reduce the gain by this factor when oscillating.
float sum_err
integration of the error for I-gain
float past_OF[COV_WINDOW_SIZE]
float setpoint
setpoint for constant divergence/flow
float D
D-gain for control.
float flowY
Flow estimate in Y direction.
float nominal_value
The nominal value of thrust, phi or theta depending on Z, Y, X.
float OF[COV_WINDOW_SIZE]
float err
Current tracking error.
float ramp
The ramp pused is increased with per dt.
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.
void vertical_ctrl_module_init(void)
Initialize the vertical optical flow hover module.
void guidance_v_module_enter(void)
Entering the vertical module (user switched to module)
static abi_event optical_flow_ev
static void send_optical_flow_hover(struct transport_tx *trans, struct link_device *dev)
void horizontal_ctrl_module_run(bool in_flight)
Run the horizontal optical flow hover module.
void guidance_v_module_run(bool in_flight)
struct OpticalFlowHoverControl of_hover_ctrl_Y
struct FloatVect3 covariances
struct Int32Vect2 of_hover_pos_err
struct Int32Vect2 of_hover_cmd_earth
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,...
void guidance_h_module_read_rc(void)
void guidance_v_module_init(void)
struct OpticalFlowHoverControl of_hover_ctrl_Z
#define OFH_HOR_X_SLOPE_B
#define OFH_HOR_X_SLOPE_A
struct OFhistory historyZ
#define OFH_OPTICAL_FLOW_ID
void guidance_h_module_run(bool in_flight)
void vertical_ctrl_module_run(bool in_flight)
Run the vertical optical flow hover module.
struct OFhistory historyX
#define OFH_COVFLOW_SETPOINT
static void reset_vertical_vars(void)
Reset all vertical variables:
void optical_flow_hover_init()
void horizontal_ctrl_module_init(void)
Initialize the horizontal optical flow hover module.
void guidance_h_module_init(void)
struct Int32Vect2 of_hover_ref_pos
struct OFhistory historyY
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
struct Int32Vect2 of_hover_trim_att_integrator
#define OFH_HOR_Y_SLOPE_B
void guidance_h_module_enter(void)
Entering the horizontal module (user switched to module)
#define OFH_HOR_Y_SLOPE_A
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
#define GUIDANCE_H_MAX_BANK
Max bank controlled by guidance.
#define GUIDANCE_V_MODE_MODULE
General attitude stabilization interface for rotorcrafts.
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
void stabilization_attitude_run(bool in_flight)
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
General stabilization interface for rotorcrafts.
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
static const struct usb_device_descriptor dev
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
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.