Guidance for the obstacle avoidance methods. More...

#include "guidance_OA.h"
#include <stdio.h>
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "autopilot.h"
#include "subsystems/datalink/downlink.h"
#include "subsystems/datalink/telemetry.h"
#include "subsystems/electrical.h"
#include "std.h"

Include dependency graph for guidance_OA.c:

Go to the source code of this file.

Macros
#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
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...

void	OA_update ()
	Update the controls based on a vision result. More...

Variables
struct opticflow_stab_t	opticflow_stab

int8_t	filter_flag = 0

int8_t	repulsionforce_filter_flag = 0

oa_method	OA_method_flag

int8_t	opti_speed_flag = 1

float	vref_max = 100

float	ref_pitch = 0.0

float	ref_roll = 0.0

float	r_dot_new = 0.0

float	desired_vx = 0.0

float	desired_vy = 0.0

float	speed_pot = 0.0

float	yaw_diff = 0.0

float	alpha_fil = 1.0

float	heading_target = 0

float	new_heading = 0

float	v_desired = 0.0

int32_t	yaw_rate = 0

float	yaw_rate_write = 0

float	yaw_ref_write = 0

int32_t	keep_yaw_rate = 0

int32_t	keep_turning = 0

float	err_vx = 0

float	err_vy = 0

float	Total_Kan_x = 0

float	Total_Kan_y = 0

struct NedCoor_f	opti_speed_read

struct FloatVect3	Total_force = {0, 0, 0}

Detailed Description

Guidance for the obstacle avoidance methods.

Definition in file guidance_OA.c.

Macro Definition Documentation

#define CMD_OF_SAT 1500

Definition at line 41 of file guidance_OA.c.

Referenced by OA_update().

#define VISION_DESIRED_VX 0

Definition at line 65 of file guidance_OA.c.

#define VISION_DESIRED_VY 0

Definition at line 70 of file guidance_OA.c.

#define VISION_PHI_IGAIN 20

Definition at line 50 of file guidance_OA.c.

#define VISION_PHI_PGAIN 400

Definition at line 45 of file guidance_OA.c.

#define VISION_THETA_IGAIN 20

Definition at line 60 of file guidance_OA.c.

#define VISION_THETA_PGAIN 400

Definition at line 55 of file guidance_OA.c.

Function Documentation

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 147 of file guidance_OA.c.

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

Here is the call graph for this function:

void guidance_h_module_init ( void )

Initialization of horizontal guidance module.

Definition at line 138 of file guidance_OA.c.

References ctrl_module_init().

Here is the call graph for this function:

void guidance_h_module_read_rc ( void )

Read the RC commands.

Definition at line 166 of file guidance_OA.c.

References ctrl_module_demo, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, RADIO_YAW, ctrl_module_demo_struct::rc_t, ctrl_module_demo_struct::rc_x, ctrl_module_demo_struct::rc_y, ctrl_module_demo_struct::rc_z, and RadioControl::values.

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 175 of file guidance_OA.c.

References opticflow_stab_t::cmd, ctrl_module_run(), OA_update(), stabilization_attitude_run(), and stabilization_attitude_set_rpy_setpoint_i().

Here is the call graph for this function:

void OA_update ( void )

Update the controls based on a vision result.

Parameters

[in] *result The opticflow calculation result used for control

Definition at line 189 of file guidance_OA.c.

References ANGLE_BFP_OF_REAL, opticflow_stab_t::cmd, CMD_OF_SAT, opticflow_stab_t::desired_vx, opticflow_stab_t::desired_vy, opticflow_stab_t::err_vx_int, opticflow_stab_t::err_vy_int, INT32_ANGLE_NORMALIZE, LOGICBASED, NO_OBSTACLE_AVOIDANCE, opti_speed_read, Int32Eulers::phi, opticflow_stab_t::phi_igain, opticflow_stab_t::phi_pgain, PINGPONG, POT_HEADING, FloatEulers::psi, Int32Eulers::psi, r_dot_new, ref_pitch, ref_roll, SAFETYZONE, speed_pot, stateGetNedToBodyEulers_f(), stateGetNedToBodyEulers_i(), stateGetSpeedNed_f(), Int32Eulers::theta, opticflow_stab_t::theta_igain, opticflow_stab_t::theta_pgain, Total_Kan_x, Total_Kan_y, FloatVect2::x, NedCoor_f::x, FloatVect2::y, NedCoor_f::y, and yaw_rate.

Referenced by guidance_h_module_run().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

float alpha_fil = 1.0

Definition at line 112 of file guidance_OA.c.

Referenced by CN_potential_velocity().

float desired_vx = 0.0

Definition at line 108 of file guidance_OA.c.

float desired_vy = 0.0

Definition at line 109 of file guidance_OA.c.

float err_vx = 0

Definition at line 123 of file guidance_OA.c.

Referenced by stabilization_opticflow_vel_cb().

float err_vy = 0

Definition at line 124 of file guidance_OA.c.

Referenced by stabilization_opticflow_vel_cb().

int8_t filter_flag = 0

Definition at line 93 of file guidance_OA.c.

float heading_target = 0

Definition at line 113 of file guidance_OA.c.

int32_t keep_turning = 0

Definition at line 121 of file guidance_OA.c.

int32_t keep_yaw_rate = 0

Definition at line 120 of file guidance_OA.c.

float new_heading = 0

Definition at line 114 of file guidance_OA.c.

Referenced by CN_escape_velocity(), CN_potential_velocity(), CN_vector_escape_velocity(), and increase_nav_heading().

oa_method OA_method_flag

Initial value:

=

PINGPONG

Definition: guidance_OA.h:52

Definition at line 96 of file guidance_OA.c.

Referenced by CN_calculate_target(), and serial_update().

int8_t opti_speed_flag = 1

Definition at line 98 of file guidance_OA.c.

struct NedCoor_f opti_speed_read

Definition at line 129 of file guidance_OA.c.

Referenced by OA_update().

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 83 of file guidance_OA.c.

float r_dot_new = 0.0

Definition at line 107 of file guidance_OA.c.

Referenced by CN_potential_heading(), CN_potential_velocity(), and OA_update().

float ref_pitch = 0.0

Definition at line 103 of file guidance_OA.c.

Referenced by CN_escape_velocity(), CN_potential_velocity(), CN_vector_escape_velocity(), CN_vector_velocity(), OA_update(), and pingpong_euler().

float ref_roll = 0.0

Definition at line 104 of file guidance_OA.c.

Referenced by CN_escape_velocity(), CN_vector_escape_velocity(), CN_vector_velocity(), OA_update(), and pingpong_euler().

int8_t repulsionforce_filter_flag = 0

Definition at line 94 of file guidance_OA.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

float speed_pot = 0.0

Definition at line 110 of file guidance_OA.c.

Referenced by CN_escape_velocity(), CN_potential_heading(), CN_potential_velocity(), CN_vector_escape_velocity(), and OA_update().

struct FloatVect3 Total_force = {0, 0, 0}

Definition at line 130 of file guidance_OA.c.

float Total_Kan_x = 0

Definition at line 126 of file guidance_OA.c.

Referenced by OA_update().

float Total_Kan_y = 0

Definition at line 127 of file guidance_OA.c.

Referenced by OA_update().

float v_desired = 0.0

Definition at line 115 of file guidance_OA.c.

float vref_max = 100

Definition at line 99 of file guidance_OA.c.

Referenced by CN_escape_velocity(), CN_potential_heading(), CN_potential_velocity(), and CN_vector_escape_velocity().

float yaw_diff = 0.0

Definition at line 111 of file guidance_OA.c.

int32_t yaw_rate = 0

Definition at line 117 of file guidance_OA.c.

Referenced by OA_update().

float yaw_rate_write = 0

Definition at line 118 of file guidance_OA.c.

float yaw_ref_write = 0

Definition at line 119 of file guidance_OA.c.

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

Function Documentation

Variable Documentation