Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 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_t in_flight)
 
static void guidance_h_hover_enter (void)
 
static void guidance_h_nav_enter (void)
 
static void transition_run (void)
 
static void read_rc_setpoint_speed_i (struct Int32Vect2 *speed_sp, bool_t in_flight)
 read speed setpoint from RC More...
 
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_t in_flight)
 
void guidance_h_run (bool_t in_flight)
 

Variables

uint8_t guidance_h_mode
 
bool_t guidance_h_use_ref
 
bool_t guidance_h_approx_force_by_thrust
 
struct Int32Vect2 guidance_h_pos_sp
 horizontal position setpoint in NED. More...
 
struct Int32Vect2 guidance_h_pos_ref
 with INT32_POS_FRAC More...
 
struct Int32Vect2 guidance_h_speed_ref
 with INT32_SPEED_FRAC More...
 
struct Int32Vect2 guidance_h_accel_ref
 with INT32_ACCEL_FRAC More...
 
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...
 
struct Int32Eulers guidance_h_rc_sp
 with INT32_ANGLE_FRAC More...
 
int32_t guidance_h_heading_sp
 with INT32_ANGLE_FRAC More...
 
int32_t guidance_h_pgain
 
int32_t guidance_h_dgain
 
int32_t guidance_h_igain
 
int32_t guidance_h_again
 
int32_t guidance_h_vgain
 
int32_t transition_percentage
 
int32_t transition_theta_offset
 

Detailed Description

Horizontal guidance for rotorcrafts.

Definition in file guidance_h.c.

Macro Definition Documentation

#define GH_GAIN_SCALE   2

Definition at line 420 of file guidance_h.c.

Referenced by guidance_h_traj_run().

#define GUIDANCE_H_AGAIN   0

Definition at line 41 of file guidance_h.c.

Referenced by guidance_h_init().

#define GUIDANCE_H_APPROX_FORCE_BY_THRUST   FALSE

Definition at line 65 of file guidance_h.c.

Referenced by guidance_h_init().

#define GUIDANCE_H_MAX_BANK   RadOfDeg(20)

Definition at line 58 of file guidance_h.c.

Referenced by guidance_h_traj_run().

#define GUIDANCE_H_THRUST_CMD_FILTER   10

Definition at line 415 of file guidance_h.c.

Referenced by guidance_h_traj_run().

#define GUIDANCE_H_VGAIN   0

Definition at line 45 of file guidance_h.c.

Referenced by guidance_h_init().

#define MAX_POS_ERR   POS_BFP_OF_REAL(16.)

Definition at line 411 of file guidance_h.c.

Referenced by guidance_h_traj_run().

#define MAX_SPEED_ERR   SPEED_BFP_OF_REAL(16.)

Definition at line 412 of file guidance_h.c.

Referenced by guidance_h_traj_run().

Function Documentation

static void guidance_h_hover_enter ( void  )
static

Definition at line 486 of file guidance_h.c.

References guidance_h_pos_sp, guidance_h_rc_sp, Int32Eulers::psi, reset_guidance_reference_from_current_position(), stateGetNedToBodyEulers_i(), stateGetPositionNed_i(), and VECT2_COPY.

Referenced by guidance_h_mode_changed(), and guidance_h_run().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void guidance_h_init ( void  )

Definition at line 163 of file guidance_h.c.

References GUIDANCE_H_AGAIN, GUIDANCE_H_APPROX_FORCE_BY_THRUST, GUIDANCE_H_MODE_KILL, guidance_h_pos_sp, guidance_h_rc_sp, guidance_h_trim_att_integrator, GUIDANCE_H_USE_REF, GUIDANCE_H_VGAIN, INT_EULERS_ZERO, INT_VECT2_ZERO, and register_periodic_telemetry().

Referenced by autopilot_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void guidance_h_nav_enter ( void  )
static

Definition at line 496 of file guidance_h.c.

References guidance_h_pos_sp, INT32_VECT2_NED_OF_ENU, nav_heading, navigation_carrot, Int32Eulers::psi, reset_guidance_reference_from_current_position(), and stateGetNedToBodyEulers_i().

Referenced by guidance_h_mode_changed(), and guidance_h_run().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void read_rc_setpoint_speed_i ( struct Int32Vect2 speed_sp,
bool_t  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 517 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

Definition at line 190 of file guidance_h.c.

References gh_set_ref(), guidance_h_accel_ref, guidance_h_pos_ref, guidance_h_speed_ref, guidance_h_trim_att_integrator, INT_VECT2_ZERO, stateGetPositionNed_i(), stateGetSpeedNed_i(), and VECT2_COPY.

Referenced by guidance_h_hover_enter(), and guidance_h_nav_enter().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void transition_run ( void  )
inlinestatic

Definition at line 506 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

int32_t guidance_h_again

Definition at line 91 of file guidance_h.c.

bool_t guidance_h_approx_force_by_thrust

Definition at line 71 of file guidance_h.c.

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

Referenced by guidance_h_run(), and guidance_h_traj_run().

int32_t guidance_h_dgain

Definition at line 89 of file guidance_h.c.

int32_t guidance_h_heading_sp

with INT32_ANGLE_FRAC

Definition at line 86 of file guidance_h.c.

Referenced by send_fp().

int32_t guidance_h_igain

Definition at line 90 of file guidance_h.c.

uint8_t guidance_h_mode

Definition at line 69 of file guidance_h.c.

Referenced by send_status().

int32_t guidance_h_pgain

Definition at line 88 of file guidance_h.c.

struct Int32Vect2 guidance_h_pos_err

Definition at line 80 of file guidance_h.c.

Referenced by guidance_h_traj_run().

struct Int32Vect2 guidance_h_pos_sp

horizontal position setpoint in NED.

fixed point representation: Q23.8 accuracy 0.0039, range 8388km

Definition at line 73 of file guidance_h.c.

Referenced by guidance_h_hover_enter(), guidance_h_init(), guidance_h_nav_enter(), guidance_h_run(), guidance_h_update_reference(), and send_fp().

struct Int32Eulers guidance_h_rc_sp
struct Int32Vect2 guidance_h_speed_err

Definition at line 81 of file guidance_h.c.

Referenced by guidance_h_traj_run().

struct Int32Vect2 guidance_h_trim_att_integrator
bool_t guidance_h_use_ref

Definition at line 70 of file guidance_h.c.

int32_t guidance_h_vgain

Definition at line 92 of file guidance_h.c.

int32_t transition_percentage

Definition at line 94 of file guidance_h.c.

int32_t transition_theta_offset

Definition at line 95 of file guidance_h.c.

Referenced by stabilization_attitude_read_rc_roll_pitch_earth_quat_f().