guidance_opticflow_hover.c File Reference

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

#include "guidance_opticflow_hover.h"
#include "subsystems/abi.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "autopilot.h"
#include "subsystems/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, float vel_x, float vel_y, float vel_z, float noise_x, float noise_y, float noise_z)
	Update the controls on a new VELOCITY_ESTIMATE ABI message. More...
void	guidance_h_module_init (void)
	Initialization of horizontal guidance module. More...
void	guidance_h_module_enter (void)
	Horizontal guidance mode enter resets the errors and starts the controller. More...
void	guidance_h_module_read_rc (void)
	Read the RC commands. More...
void	guidance_h_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_h_module_enter()

void guidance_h_module_enter

(

void

)

Horizontal guidance mode enter resets the errors and starts the controller.

Entering the horizontal 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, opticflow_stab, Int32Eulers::phi, Int32Eulers::psi, stateGetNedToBodyEulers_i(), and Int32Eulers::theta.

Here is the call graph for this function:

guidance_h_module_init()

void guidance_h_module_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:

guidance_h_module_read_rc()

void guidance_h_module_read_rc

(

void

)

Read the RC commands.

Definition at line 130 of file guidance_opticflow_hover.c.

guidance_h_module_run()

void guidance_h_module_run

(

bool

in_flight

)

Main guidance loop.

Parameters

[in]

in_flight

Whether we are in flight or not

Definition at line 139 of file guidance_opticflow_hover.c.

References opticflow_stab_t::cmd, opticflow_stab, stabilization_attitude_run(), and stabilization_attitude_set_rpy_setpoint_i().

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, float  vel_x, float  vel_y, float  vel_z, float  noise_x, float  noise_y, float  noise_z  )

static

Update the controls on a new VELOCITY_ESTIMATE ABI message.

Definition at line 151 of file guidance_opticflow_hover.c.

References AP_MODE_MODULE, autopilot_get_mode(), 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, f, 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_h_module_init().

Here is the call graph for this function:

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 90 of file guidance_opticflow_hover.c.

Referenced by guidance_h_module_enter(), guidance_h_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_h_module_init().