Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
guidance_h.c File Reference
+ Include dependency graph for guidance_h.c:

Go to the source code of this file.

Macros

#define GUIDANCE_H_AGAIN   0
 
#define GUIDANCE_H_VGAIN   0
 
#define GUIDANCE_H_MAX_BANK   RadOfDeg(20)
 
#define GUIDANCE_H_APPROX_FORCE_BY_THRUST   FALSE
 
#define GUIDANCE_INDI   FALSE
 
#define GUIDANCE_HEADING_IS_FREE   TRUE
 
#define MAX_POS_ERR   POS_BFP_OF_REAL(16.)
 
#define MAX_SPEED_ERR   SPEED_BFP_OF_REAL(16.)
 
#define GUIDANCE_H_THRUST_CMD_FILTER   10
 
#define GH_GAIN_SCALE   2
 

Functions

static void guidance_h_update_reference (void)
 
static void guidance_h_traj_run (bool in_flight)
 
static void transition_run (bool to_forward)
 
static void read_rc_setpoint_speed_i (struct Int32Vect2 *speed_sp, bool in_flight)
 read speed setpoint from RC More...
 
static void send_gh (struct transport_tx *trans, struct link_device *dev)
 
static void send_hover_loop (struct transport_tx *trans, struct link_device *dev)
 
static void send_href (struct transport_tx *trans, struct link_device *dev)
 
static void send_tune_hover (struct transport_tx *trans, struct link_device *dev)
 
void guidance_h_init (void)
 
static void reset_guidance_reference_from_current_position (void)
 
void guidance_h_mode_changed (uint8_t new_mode)
 
void guidance_h_read_rc (bool in_flight)
 
void guidance_h_run (bool in_flight)
 
void guidance_h_hover_enter (void)
 
void guidance_h_nav_enter (void)
 
void guidance_h_from_nav (bool in_flight)
 Set horizontal guidance from NAV and run control loop. More...
 
void guidance_h_set_igain (uint32_t igain)
 
void guidance_h_guided_run (bool in_flight)
 Run GUIDED mode control. More...
 
void guidance_h_set_pos (float x, float y)
 Set horizontal position setpoint. More...
 
void guidance_h_set_heading (float heading)
 Set heading setpoint. More...
 
void guidance_h_set_body_vel (float vx, float vy)
 Set body relative horizontal velocity setpoint. More...
 
void guidance_h_set_vel (float vx, float vy)
 Set horizontal velocity setpoint. More...
 
void guidance_h_set_heading_rate (float rate)
 Set heading rate setpoint. More...
 
const struct Int32Vect2guidance_h_get_pos_err (void)
 Gets the position error. More...
 

Variables

struct HorizontalGuidance guidance_h
 
int32_t transition_percentage
 
struct Int32Vect2 guidance_h_pos_err
 
struct Int32Vect2 guidance_h_speed_err
 
struct Int32Vect2 guidance_h_trim_att_integrator
 
struct Int32Vect2 guidance_h_cmd_earth
 horizontal guidance command. More...
 

Detailed Description

Horizontal guidance for rotorcrafts.

Definition in file guidance_h.c.

Macro Definition Documentation

◆ GH_GAIN_SCALE

#define GH_GAIN_SCALE   2

Definition at line 483 of file guidance_h.c.

◆ GUIDANCE_H_AGAIN

#define GUIDANCE_H_AGAIN   0

Definition at line 53 of file guidance_h.c.

◆ GUIDANCE_H_APPROX_FORCE_BY_THRUST

#define GUIDANCE_H_APPROX_FORCE_BY_THRUST   FALSE

Definition at line 77 of file guidance_h.c.

◆ GUIDANCE_H_MAX_BANK

#define GUIDANCE_H_MAX_BANK   RadOfDeg(20)

Definition at line 70 of file guidance_h.c.

◆ GUIDANCE_H_THRUST_CMD_FILTER

#define GUIDANCE_H_THRUST_CMD_FILTER   10

Definition at line 478 of file guidance_h.c.

◆ GUIDANCE_H_VGAIN

#define GUIDANCE_H_VGAIN   0

Definition at line 57 of file guidance_h.c.

◆ GUIDANCE_HEADING_IS_FREE

#define GUIDANCE_HEADING_IS_FREE   TRUE

Definition at line 87 of file guidance_h.c.

◆ GUIDANCE_INDI

#define GUIDANCE_INDI   FALSE

Definition at line 81 of file guidance_h.c.

◆ MAX_POS_ERR

#define MAX_POS_ERR   POS_BFP_OF_REAL(16.)

Definition at line 474 of file guidance_h.c.

◆ MAX_SPEED_ERR

#define MAX_SPEED_ERR   SPEED_BFP_OF_REAL(16.)

Definition at line 475 of file guidance_h.c.

Function Documentation

◆ guidance_h_from_nav()

◆ guidance_h_get_pos_err()

const struct Int32Vect2* guidance_h_get_pos_err ( void  )

Gets the position error.

Parameters
none.
Returns
Pointer to a structure containing x and y position errors

Definition at line 754 of file guidance_h.c.

References guidance_h_pos_err.

◆ guidance_h_guided_run()

void guidance_h_guided_run ( bool  in_flight)

◆ guidance_h_hover_enter()

◆ guidance_h_init()

◆ guidance_h_mode_changed()

◆ guidance_h_nav_enter()

void guidance_h_nav_enter ( void  )

◆ guidance_h_read_rc()

◆ guidance_h_run()

◆ guidance_h_set_body_vel()

void guidance_h_set_body_vel ( float  vx,
float  vy 
)

Set body relative horizontal velocity setpoint.

Parameters
vxforward velocity (body frame) in meters/sec.
vyright velocity (body frame) in meters/sec.

Definition at line 733 of file guidance_h.c.

References guidance_h_set_vel(), FloatEulers::psi, Int32Eulers::psi, and stateGetNedToBodyEulers_f().

Referenced by autopilot_guided_update(), and orange_avoider_guided_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ guidance_h_set_heading()

void guidance_h_set_heading ( float  heading)

◆ guidance_h_set_heading_rate()

void guidance_h_set_heading_rate ( float  rate)

Set heading rate setpoint.

Parameters
rateHeading rate in radians.

Definition at line 748 of file guidance_h.c.

References guidance_h, HorizontalGuidanceSetpoint::heading_rate, HorizontalGuidanceSetpoint::mask, and HorizontalGuidance::sp.

Referenced by autopilot_guided_update(), and orange_avoider_guided_periodic().

+ Here is the caller graph for this function:

◆ guidance_h_set_igain()

void guidance_h_set_igain ( uint32_t  igain)

◆ guidance_h_set_pos()

void guidance_h_set_pos ( float  x,
float  y 
)

Set horizontal position setpoint.

Parameters
xNorth position (local NED frame) in meters.
yEast position (local NED frame) in meters.

Definition at line 719 of file guidance_h.c.

References guidance_h, HorizontalGuidanceSetpoint::mask, HorizontalGuidanceSetpoint::pos, POS_BFP_OF_REAL, HorizontalGuidance::sp, Int32Vect2::x, and Int32Vect2::y.

Referenced by autopilot_guided_goto_ned(), autopilot_guided_update(), guidance_h_from_nav(), guidance_h_hover_enter(), and guidance_h_nav_enter().

+ Here is the caller graph for this function:

◆ guidance_h_set_vel()

void guidance_h_set_vel ( float  vx,
float  vy 
)

Set horizontal velocity setpoint.

Parameters
vxNorth velocity (local NED frame) in meters/sec.
vyEast velocity (local NED frame) in meters/sec.

Definition at line 741 of file guidance_h.c.

References guidance_h, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, SPEED_BFP_OF_REAL, Int32Vect2::x, and Int32Vect2::y.

Referenced by autopilot_guided_move_ned(), autopilot_guided_update(), and guidance_h_set_body_vel().

+ Here is the caller graph for this function:

◆ guidance_h_traj_run()

◆ guidance_h_update_reference()

◆ read_rc_setpoint_speed_i()

static void read_rc_setpoint_speed_i ( struct Int32Vect2 speed_sp,
bool  in_flight 
)
static

read speed setpoint from RC

Todo:
calc proper scale while making sure a division by zero can't occur

Definition at line 661 of file guidance_h.c.

References GUIDANCE_H_REF_MAX_SPEED, INT32_TRIG_FRAC, MAX_PPRZ, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, radio_control, RADIO_PITCH, RADIO_ROLL, SPEED_BFP_OF_REAL, speed_sp, stateGetNedToBodyEulers_i(), RadioControl::values, FloatVect3::x, and FloatVect3::y.

Referenced by guidance_h_read_rc().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ reset_guidance_reference_from_current_position()

static void reset_guidance_reference_from_current_position ( void  )
inlinestatic

◆ send_gh()

static void send_gh ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 117 of file guidance_h.c.

References dev, guidance_h, HorizontalGuidanceReference::pos, HorizontalGuidanceSetpoint::pos, HorizontalGuidance::ref, HorizontalGuidance::sp, stateGetPositionNed_i(), NedCoor_i::x, Int32Vect2::x, NedCoor_i::y, and Int32Vect2::y.

Referenced by guidance_h_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ send_hover_loop()

static void send_hover_loop ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

◆ send_href()

static void send_href ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

◆ send_tune_hover()

static void send_tune_hover ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 159 of file guidance_h.c.

References dev, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_YAW, stabilization_cmd, stateGetNedToBodyEulers_i(), and RadioControl::values.

Referenced by guidance_h_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ transition_run()

static void transition_run ( bool  to_forward)
inlinestatic

Definition at line 643 of file guidance_h.c.

References ANGLE_BFP_OF_REAL, INT32_ANGLE_FRAC, INT32_PERCENTAGE_FRAC, INT_MULT_RSHIFT, transition_percentage, and transition_theta_offset.

Referenced by guidance_h_run().

+ Here is the caller graph for this function:

Variable Documentation

◆ guidance_h

◆ guidance_h_cmd_earth

struct Int32Vect2 guidance_h_cmd_earth

horizontal guidance command.

In north/east with INT32_ANGLE_FRAC

Todo:
convert to real force command

Definition at line 105 of file guidance_h.c.

Referenced by guidance_h_from_nav(), guidance_h_guided_run(), guidance_h_traj_run(), and send_hover_loop().

◆ guidance_h_pos_err

struct Int32Vect2 guidance_h_pos_err

Definition at line 97 of file guidance_h.c.

Referenced by guidance_h_get_pos_err(), guidance_h_traj_run(), and send_hover_loop().

◆ guidance_h_speed_err

struct Int32Vect2 guidance_h_speed_err

Definition at line 98 of file guidance_h.c.

Referenced by guidance_h_traj_run(), and send_hover_loop().

◆ guidance_h_trim_att_integrator

◆ transition_percentage

int32_t transition_percentage