Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_h.c File Reference

Horizontal guidance for rotorcrafts. More...

+ 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 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...
 
bool guidance_h_set_guided_pos (float x, float y)
 Set horizontal position setpoint in GUIDED mode. More...
 
bool guidance_h_set_guided_heading (float heading)
 Set heading setpoint in GUIDED mode. More...
 
bool guidance_h_set_guided_body_vel (float vx, float vy)
 Set body relative horizontal velocity setpoint in GUIDED mode. More...
 
bool guidance_h_set_guided_vel (float vx, float vy)
 Set horizontal velocity setpoint in GUIDED mode. More...
 
bool guidance_h_set_guided_heading_rate (float rate)
 Set heading rate setpoint in GUIDED mode. More...
 
const struct Int32Vect2guidance_h_get_pos_err (void)
 Gets the position error. More...
 

Variables

struct HorizontalGuidance guidance_h
 
int32_t transition_percentage
 
int32_t transition_theta_offset
 
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

#define GH_GAIN_SCALE   2

Definition at line 451 of file guidance_h.c.

Referenced by guidance_h_traj_run().

#define GUIDANCE_H_AGAIN   0

Definition at line 49 of file guidance_h.c.

Referenced by guidance_h_init().

#define GUIDANCE_H_APPROX_FORCE_BY_THRUST   FALSE

Definition at line 73 of file guidance_h.c.

Referenced by guidance_h_init().

#define GUIDANCE_H_MAX_BANK   RadOfDeg(20)

Definition at line 66 of file guidance_h.c.

Referenced by guidance_h_traj_run(), and guidance_indi_run().

#define GUIDANCE_H_THRUST_CMD_FILTER   10

Definition at line 446 of file guidance_h.c.

Referenced by guidance_h_traj_run().

#define GUIDANCE_H_VGAIN   0

Definition at line 53 of file guidance_h.c.

Referenced by guidance_h_init().

#define GUIDANCE_INDI   FALSE

Definition at line 77 of file guidance_h.c.

#define MAX_POS_ERR   POS_BFP_OF_REAL(16.)

Definition at line 442 of file guidance_h.c.

Referenced by guidance_h_traj_run().

#define MAX_SPEED_ERR   SPEED_BFP_OF_REAL(16.)

Definition at line 443 of file guidance_h.c.

Referenced by guidance_h_traj_run().

Function Documentation

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 740 of file guidance_h.c.

References guidance_h_pos_err.

void guidance_h_guided_run ( bool  in_flight)
bool guidance_h_set_guided_body_vel ( float  vx,
float  vy 
)

Set body relative horizontal velocity setpoint in GUIDED mode.

Parameters
vxforward velocity (body frame) in meters/sec.
vyright velocity (body frame) in meters/sec.
Returns
TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)

Definition at line 711 of file guidance_h.c.

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

Referenced by autopilot_guided_update().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool guidance_h_set_guided_heading ( float  heading)

Set heading setpoint in GUIDED mode.

Parameters
headingSetpoint in radians.
Returns
TRUE if setpoint was set (currently in GUIDANCE_H_MODE_GUIDED)

Definition at line 700 of file guidance_h.c.

References FLOAT_ANGLE_NORMALIZE, guidance_h, GUIDANCE_H_MODE_GUIDED, heading, HorizontalGuidanceSetpoint::heading, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, and HorizontalGuidance::sp.

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

+ Here is the caller graph for this function:

bool guidance_h_set_guided_heading_rate ( float  rate)

Set heading rate setpoint in GUIDED mode.

Parameters
rateHeading rate in radians.
Returns
TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)

Definition at line 730 of file guidance_h.c.

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

Referenced by autopilot_guided_update().

+ Here is the caller graph for this function:

bool guidance_h_set_guided_pos ( float  x,
float  y 
)

Set horizontal position setpoint in GUIDED mode.

Parameters
xNorth position (local NED frame) in meters.
yEast position (local NED frame) in meters.
Returns
TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)

Definition at line 689 of file guidance_h.c.

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

Referenced by autopilot_guided_goto_ned(), and autopilot_guided_update().

+ Here is the caller graph for this function:

bool guidance_h_set_guided_vel ( float  vx,
float  vy 
)

Set horizontal velocity setpoint in GUIDED mode.

Parameters
vxNorth velocity (local NED frame) in meters/sec.
vyEast velocity (local NED frame) in meters/sec.
Returns
TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)

Definition at line 719 of file guidance_h.c.

References guidance_h, GUIDANCE_H_MODE_GUIDED, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, 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_guided_body_vel().

+ Here is the caller graph for this function:

void guidance_h_set_igain ( uint32_t  igain)
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 631 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, stateGetNedToBodyEulers_i(), RadioControl::values, Int32Vect2::x, and Int32Vect2::y.

Referenced by guidance_h_read_rc().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

Definition at line 108 of file guidance_h.c.

References 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:

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

Definition at line 150 of file guidance_h.c.

References 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:

static void transition_run ( bool  to_forward)
inlinestatic

Definition at line 613 of file guidance_h.c.

References ANGLE_BFP_OF_REAL, INT32_ANGLE_FRAC, INT32_PERCENTAGE_FRAC, and INT_MULT_RSHIFT.

Referenced by guidance_h_run().

+ Here is the caller graph for this function:

Variable Documentation

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 96 of file guidance_h.c.

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

struct Int32Vect2 guidance_h_pos_err

Definition at line 88 of file guidance_h.c.

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

struct Int32Vect2 guidance_h_speed_err

Definition at line 89 of file guidance_h.c.

Referenced by guidance_h_traj_run(), and send_hover_loop().

int32_t transition_percentage

Definition at line 82 of file guidance_h.c.

int32_t transition_theta_offset

Definition at line 83 of file guidance_h.c.

Referenced by stabilization_attitude_read_rc_roll_pitch_earth_quat_f().