guidance_opticflow_hover.c File Reference

Optical-flow based control for Linux based systems. More...

#include "guidance_opticflow_hover.h"
#include "modules/core/abi.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "autopilot.h"
#include "modules/datalink/downlink.h"

Include dependency graph for guidance_opticflow_hover.c:

Go to the source code of this file.

Macros
#define	VISION_VELOCITY_ESTIMATE_ID   ABI_BROADCAST
	Default sender to accect VELOCITY_ESTIMATE messages from. More...
#define	CMD_OF_SAT   1500
#define	VISION_PHI_PGAIN   400
#define	VISION_PHI_IGAIN   20
#define	VISION_THETA_PGAIN   400
#define	VISION_THETA_IGAIN   20
#define	VISION_DESIRED_VX   0
#define	VISION_DESIRED_VY   0

Functions
static void	stabilization_opticflow_vel_cb (uint8_t sender_id, uint32_t stamp UNUSED, float vel_x, float vel_y, float vel_z UNUSED, float noise_x, float noise_y, float noise_z UNUSED)
	Update the controls on a new VELOCITY_ESTIMATE ABI message. More...
void	guidance_opticflow_hover_init (void)
	Initialization of horizontal guidance module. More...
void	guidance_module_enter (void)
	guidance mode enter resets the errors and starts the controller. More...
void	guidance_module_run (bool in_flight)
	Main guidance loop. More...

Variables
static abi_event	velocity_est_ev
struct opticflow_stab_t	opticflow_stab

Detailed Description

Optical-flow based control for Linux based systems.

Control loops for optic flow based hovering. Computes setpoint for the lower level attitude stabilization to control horizontal velocity.

Definition in file guidance_opticflow_hover.c.

Macro Definition Documentation

CMD_OF_SAT

#define CMD_OF_SAT   1500

Definition at line 47 of file guidance_opticflow_hover.c.

VISION_DESIRED_VX

#define VISION_DESIRED_VX   0

Definition at line 70 of file guidance_opticflow_hover.c.

VISION_DESIRED_VY

#define VISION_DESIRED_VY   0

Definition at line 75 of file guidance_opticflow_hover.c.

VISION_PHI_IGAIN

#define VISION_PHI_IGAIN   20

Definition at line 55 of file guidance_opticflow_hover.c.

VISION_PHI_PGAIN

#define VISION_PHI_PGAIN   400

Definition at line 50 of file guidance_opticflow_hover.c.

VISION_THETA_IGAIN

#define VISION_THETA_IGAIN   20

Definition at line 65 of file guidance_opticflow_hover.c.

VISION_THETA_PGAIN

#define VISION_THETA_PGAIN   400

Definition at line 60 of file guidance_opticflow_hover.c.

VISION_VELOCITY_ESTIMATE_ID

#define VISION_VELOCITY_ESTIMATE_ID   ABI_BROADCAST

Default sender to accect VELOCITY_ESTIMATE messages from.

Definition at line 43 of file guidance_opticflow_hover.c.

Function Documentation

guidance_module_enter()

void guidance_module_enter

(

void

)

guidance mode enter resets the errors and starts the controller.

Entering the module (user switched to module)

Definition at line 115 of file guidance_opticflow_hover.c.

References opticflow_stab_t::cmd, opticflow_stab_t::err_vx_int, opticflow_stab_t::err_vy_int, guidance_v_mode_changed(), GUIDANCE_V_MODE_HOVER, opticflow_stab, Int32Eulers::phi, Int32Eulers::psi, stateGetNedToBodyEulers_i(), and Int32Eulers::theta.

Here is the call graph for this function:

guidance_module_run()

void guidance_module_run

(

bool

in_flight

)

Main guidance loop.

Parameters

[in]

in_flight

Whether we are in flight or not

Definition at line 133 of file guidance_opticflow_hover.c.

References Stabilization::cmd, opticflow_stab_t::cmd, guidance_v_run(), opticflow_stab, StabilizationSetpoint::sp, ThrustSetpoint::sp, stab_sp_from_eulers_i(), stabilization, and stabilization_attitude_run().

Here is the call graph for this function:

guidance_opticflow_hover_init()

void guidance_opticflow_hover_init

(

void

)

Initialization of horizontal guidance module.

Definition at line 105 of file guidance_opticflow_hover.c.

References stabilization_opticflow_vel_cb(), velocity_est_ev, and VISION_VELOCITY_ESTIMATE_ID.

Here is the call graph for this function:

stabilization_opticflow_vel_cb()

static void stabilization_opticflow_vel_cb ( uint8_t  sender_id, uint32_t stamp  UNUSED, float  vel_x, float  vel_y, float vel_z  UNUSED, float  noise_x, float  noise_y, float noise_z  UNUSED  )

static

Update the controls on a new VELOCITY_ESTIMATE ABI message.

Definition at line 144 of file guidance_opticflow_hover.c.

References opticflow_stab_t::cmd, CMD_OF_SAT, opticflow_stab_t::desired_vx, opticflow_stab_t::desired_vy, err_vx, opticflow_stab_t::err_vx_int, err_vy, opticflow_stab_t::err_vy_int, opticflow_stab, Int32Eulers::phi, opticflow_stab_t::phi_igain, opticflow_stab_t::phi_pgain, Int32Eulers::theta, opticflow_stab_t::theta_igain, and opticflow_stab_t::theta_pgain.

Referenced by guidance_opticflow_hover_init().

Here is the caller graph for this function:

Variable Documentation

opticflow_stab

struct opticflow_stab_t opticflow_stab

Initial value:

= {
  .phi_pgain = VISION_PHI_PGAIN,
  .phi_igain = VISION_PHI_IGAIN,
  .theta_pgain = VISION_THETA_PGAIN,
  .theta_igain = VISION_THETA_IGAIN,
  .desired_vx = VISION_DESIRED_VX,
  .desired_vy = VISION_DESIRED_VY
}

Definition at line 87 of file guidance_opticflow_hover.c.

Referenced by guidance_module_enter(), guidance_module_run(), and stabilization_opticflow_vel_cb().

velocity_est_ev

abi_event velocity_est_ev

static

Definition at line 87 of file guidance_opticflow_hover.c.

Referenced by guidance_opticflow_hover_init().