Paparazzi UAS
v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
|
#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"
Go to the source code of this file.
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) |
#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().
void computeOptiTrack | ( | bool | phi, |
bool | theta, | ||
struct Int32Eulers * | opti_sp_eu | ||
) |
Get the desired Euler angles for optitrack stabilization.
[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().
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.
void guidance_h_module_init | ( | void | ) |
Definition at line 625 of file optical_flow_hover.c.
References horizontal_ctrl_module_init().
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().
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.
void guidance_v_module_init | ( | void | ) |
Definition at line 619 of file optical_flow_hover.c.
References vertical_ctrl_module_init().
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().
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().
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.
Definition at line 601 of file optical_flow_hover.c.
Referenced by optical_flow_hover_init().
void optical_flow_hover_init | ( | void | ) |
Definition at line 259 of file optical_flow_hover.c.
References OpticalFlowHoverControl::cov_setpoint, GainsPID::D, DefaultPeriodic, GainsPID::I, of_hover_ctrl_X, of_hover_ctrl_Y, of_hover_ctrl_Z, OFH_COV_METHOD, OFH_COVDIV_SETPOINT, OFH_COVFLOW_SETPOINT, OFH_DGAINX, OFH_DGAINY, OFH_DGAINZ, OFH_HOVER_METHOD, OFH_IGAINX, OFH_IGAINY, OFH_IGAINZ, ofh_optical_flow_cb(), OFH_OPTICAL_FLOW_ID, OFH_OSCPHI, OFH_OSCTHETA, OFH_PGAINX, OFH_PGAINY, OFH_PGAINZ, OFH_RAMPXY, OFH_RAMPZ, OFH_REDUCTIONXY, OFH_REDUCTIONZ, GainsPID::P, OpticalFlowHoverControl::PID, OpticalFlowHoverControl::ramp, OpticalFlowHoverControl::reduction_factor, register_periodic_telemetry(), reset_horizontal_vars(), reset_vertical_vars(), send_optical_flow_hover(), and OpticalFlowHoverControl::setpoint.
|
static |
Reset all horizontal variables:
Definition at line 322 of file optical_flow_hover.c.
References cov_array_filledXY, COV_WINDOW_SIZE, covariances, GainsPID::d_err, des_inputs, FLOAT_OF_BFP, OpticalFlowHover::flowX, OpticalFlowHover::flowY, get_sys_time_float(), GUIDANCE_V_MODE_MODULE, GUIDANCE_V_MODE_MODULE_SETTING, historyX, historyY, GainsPID::I, ind_histXY, OFhistory::input, INT32_ANGLE_FRAC, int32_eulers_of_quat(), OpticalFlowHoverControl::nominal_value, OFhistory::OF, of_hover, of_hover_ctrl_X, of_hover_ctrl_Y, OFH_IGAINX, OFH_IGAINY, OFH_PGAINX, OFH_PGAINY, ofh_sp_eu, GainsPID::P, OFhistory::past_OF, DesiredInputs::phi, Int32Eulers::phi, OpticalFlowHoverControl::PID, GainsPID::previous_err, stab_att_sp_quat, GainsPID::sum_err, DesiredInputs::theta, Int32Eulers::theta, vision_time, FloatVect3::x, and FloatVect3::y.
Referenced by guidance_h_module_enter(), horizontal_ctrl_module_init(), and optical_flow_hover_init().
|
static |
Reset all vertical variables:
Definition at line 395 of file optical_flow_hover.c.
References cov_array_filledZ, COV_WINDOW_SIZE, covariances, GainsPID::d_err, OpticalFlowHover::divergence, get_sys_time_float(), historyZ, ind_histZ, OFhistory::input, OFhistory::OF, of_hover, of_hover_ctrl_Z, OFH_PGAINZ, GainsPID::P, OFhistory::past_OF, OpticalFlowHoverControl::PID, GainsPID::previous_err, GainsPID::sum_err, vision_time, and FloatVect3::z.
Referenced by guidance_v_module_enter(), optical_flow_hover_init(), and vertical_ctrl_module_init().
|
static |
Definition at line 249 of file optical_flow_hover.c.
References covariances, des_inputs, OpticalFlowHover::divergence, OpticalFlowHover::flowX, OpticalFlowHover::flowY, of_hover, of_hover_ctrl_X, of_hover_ctrl_Y, of_hover_ctrl_Z, GainsPID::P, DesiredInputs::phi, OpticalFlowHoverControl::PID, GainsPID::sum_err, DesiredInputs::theta, DesiredInputs::thrust, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by optical_flow_hover_init().
void vertical_ctrl_module_init | ( | void | ) |
Initialize the vertical optical flow hover module.
Initialize the optical flow landing module.
Definition at line 304 of file optical_flow_hover.c.
References reset_vertical_vars().
Referenced by guidance_v_module_init().
void vertical_ctrl_module_run | ( | bool | in_flight | ) |
Run the vertical optical flow hover module.
Run the optical flow landing module.
Definition at line 521 of file optical_flow_hover.c.
References OpticalFlowHoverControl::cov_setpoint, covariances, des_inputs, OpticalFlowHover::divergence, GainsPID::err, historyZ, GainsPID::I, of_hover, of_hover_ctrl_X, of_hover_ctrl_Y, of_hover_ctrl_Z, OF_LP_CONST, OFH_HOR_X_SLOPE_A, OFH_HOR_X_SLOPE_B, OFH_HOR_Y_SLOPE_A, OFH_HOR_Y_SLOPE_B, OFH_IGAINX, OFH_IGAINY, OFH_PGAINX, OFH_REDUCTIONXY, OFH_VER_SLOPE_A, OFH_VER_SLOPE_B, GainsPID::P, OpticalFlowHoverControl::PID, PID_divergence_control(), prev_vision_timeZ, OpticalFlowHoverControl::ramp, OpticalFlowHoverControl::reduction_factor, set_cov_div(), OpticalFlowHoverControl::setpoint, stabilization_cmd, GainsPID::sum_err, DesiredInputs::thrust, vision_time, and FloatVect3::z.
Referenced by guidance_v_module_run().
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)
Definition at line 213 of file optical_flow_hover.c.
struct FloatVect3 covariances |
Definition at line 181 of file optical_flow_hover.c.
Referenced by horizontal_ctrl_module_run(), reset_horizontal_vars(), reset_vertical_vars(), send_optical_flow_hover(), and vertical_ctrl_module_run().
bool derotated |
Definition at line 212 of file optical_flow_hover.c.
struct DesiredInputs des_inputs |
Definition at line 180 of file optical_flow_hover.c.
Referenced by guidance_v_module_enter(), horizontal_ctrl_module_run(), reset_horizontal_vars(), send_optical_flow_hover(), and vertical_ctrl_module_run().
float divergence_vision |
Definition at line 197 of file optical_flow_hover.c.
int16_t flowX |
Definition at line 188 of file optical_flow_hover.c.
int16_t flowY |
Definition at line 189 of file optical_flow_hover.c.
struct OFhistory historyX |
Definition at line 190 of file optical_flow_hover.c.
Referenced by horizontal_ctrl_module_run(), and reset_horizontal_vars().
struct OFhistory historyY |
Definition at line 191 of file optical_flow_hover.c.
Referenced by horizontal_ctrl_module_run(), and reset_horizontal_vars().
struct OFhistory historyZ |
Definition at line 199 of file optical_flow_hover.c.
Referenced by reset_vertical_vars(), and vertical_ctrl_module_run().
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.
Definition at line 214 of file optical_flow_hover.c.
struct Int32Vect2 of_hover_cmd_earth |
Definition at line 245 of file optical_flow_hover.c.
Referenced by computeOptiTrack().
struct OpticalFlowHoverControl of_hover_ctrl_X |
Definition at line 205 of file optical_flow_hover.c.
Referenced by horizontal_ctrl_module_run(), optical_flow_hover_init(), reset_horizontal_vars(), send_optical_flow_hover(), and vertical_ctrl_module_run().
struct OpticalFlowHoverControl of_hover_ctrl_Y |
Definition at line 206 of file optical_flow_hover.c.
Referenced by horizontal_ctrl_module_run(), optical_flow_hover_init(), reset_horizontal_vars(), send_optical_flow_hover(), and vertical_ctrl_module_run().
struct OpticalFlowHoverControl of_hover_ctrl_Z |
Definition at line 207 of file optical_flow_hover.c.
Referenced by guidance_v_module_enter(), optical_flow_hover_init(), reset_vertical_vars(), send_optical_flow_hover(), and vertical_ctrl_module_run().
struct Int32Vect2 of_hover_pos_err |
Definition at line 241 of file optical_flow_hover.c.
Referenced by computeOptiTrack().
struct Int32Vect2 of_hover_ref_pos |
Definition at line 243 of file optical_flow_hover.c.
Referenced by computeOptiTrack(), and guidance_h_module_enter().
struct Int32Vect2 of_hover_speed_err |
Definition at line 242 of file optical_flow_hover.c.
Referenced by computeOptiTrack().
struct Int32Vect2 of_hover_trim_att_integrator |
Definition at line 244 of file optical_flow_hover.c.
Referenced by computeOptiTrack().
struct Int32Eulers ofh_sp_eu |
Definition at line 194 of file optical_flow_hover.c.
Referenced by guidance_h_module_enter(), horizontal_ctrl_module_run(), and reset_horizontal_vars().
|
static |
Definition at line 202 of file optical_flow_hover.c.
bool oscillatingX |
Definition at line 186 of file optical_flow_hover.c.
bool oscillatingY |
Definition at line 187 of file optical_flow_hover.c.
bool oscillatingZ |
Definition at line 196 of file optical_flow_hover.c.
bool oscphi |
Definition at line 210 of file optical_flow_hover.c.
bool osctheta |
Definition at line 211 of file optical_flow_hover.c.
float prev_vision_timeXY |
Definition at line 184 of file optical_flow_hover.c.
Referenced by horizontal_ctrl_module_run().
float prev_vision_timeZ |
Definition at line 184 of file optical_flow_hover.c.
Referenced by vertical_ctrl_module_run().
float vision_time |
Definition at line 184 of file optical_flow_hover.c.
Referenced by horizontal_ctrl_module_run(), reset_horizontal_vars(), reset_vertical_vars(), and vertical_ctrl_module_run().