Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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_RC_ID   ABI_BROADCAST
#define GUIDANCE_H_SP_MAX_R   60.f
#define GUIDANCE_H_DEADBAND_R   200
#define YAW_DEADBAND_EXCEEDED(_rc)

Functions

static void guidance_h_update_reference (void)
static void rc_cb (uint8_t sender_id UNUSED, struct RadioControl *rc)
static void send_gh (struct transport_tx *trans, struct link_device *dev)
static void send_href (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)
static void read_rc_setpoint_heading (struct HorizontalGuidanceRCInput *rc_sp, bool in_flight, struct RadioControl *rc)
static void read_rc_setpoint_speed_i (struct Int32Vect2 *speed_sp, bool in_flight, struct RadioControl *rc)
 read speed setpoint from RC
struct StabilizationSetpoint guidance_h_run (bool in_flight)
void guidance_h_hover_enter (void)
void guidance_h_nav_enter (void)
struct StabilizationSetpoint guidance_h_from_nav (bool in_flight)
 Set horizontal guidance from NAV and run control loop.
struct StabilizationSetpoint guidance_h_guided_run (bool in_flight)
 Run GUIDED mode control.
void guidance_h_set_pos (float x, float y)
 Set horizontal position setpoint.
void guidance_h_set_heading (float heading)
 Set heading setpoint.
void guidance_h_set_body_vel (float vx, float vy)
 Set body relative horizontal velocity setpoint.
void guidance_h_set_vel (float vx, float vy)
 Set horizontal velocity setpoint.
void guidance_h_set_body_acc (float ax, float ay)
 Set body relative horizontal acceleration setpoint.
void guidance_h_set_acc (float ax, float ay)
 Set horizontal acceleration setpoint.
void guidance_h_set_heading_rate (float rate)
 Set heading rate setpoint.
void guidance_h_set_all (float x, float y, float vx, float vy, float ax, float ay)
 Set position speed and acceleration (NED frame).

Variables

struct HorizontalGuidance guidance_h
struct StabilizationSetpoint guidance_h_cmd
 horizontal guidance command.
static abi_event rc_ev

Detailed Description

Horizontal guidance for rotorcrafts.

Definition in file guidance_h.c.

Macro Definition Documentation

◆ GUIDANCE_H_DEADBAND_R

#define GUIDANCE_H_DEADBAND_R   200

Definition at line 160 of file guidance_h.c.

◆ GUIDANCE_H_RC_ID

#define GUIDANCE_H_RC_ID   ABI_BROADCAST

Definition at line 55 of file guidance_h.c.

Referenced by guidance_h_init().

◆ GUIDANCE_H_SP_MAX_R

#define GUIDANCE_H_SP_MAX_R   60.f

Definition at line 155 of file guidance_h.c.

Referenced by read_rc_setpoint_heading().

◆ YAW_DEADBAND_EXCEEDED

#define YAW_DEADBAND_EXCEEDED ( _rc)
Value:
(rc->values[RADIO_YAW] > GUIDANCE_H_DEADBAND_R || \
#define GUIDANCE_H_DEADBAND_R
Definition guidance_h.c:160

Definition at line 163 of file guidance_h.c.

Referenced by read_rc_setpoint_heading().

Function Documentation

◆ guidance_h_from_nav()

struct StabilizationSetpoint guidance_h_from_nav ( bool in_flight)

Set horizontal guidance from NAV and run control loop.

Definition at line 391 of file guidance_h.c.

References guidance_h_from_nav(), guidance_h_nav_enter(), if(), nav, NAV_HORIZONTAL_MODE_ATTITUDE, NAV_HORIZONTAL_MODE_NONE, and stab_sp_from_eulers_f().

Referenced by guidance_h_from_nav(), and guidance_h_run().

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

◆ guidance_h_guided_run()

struct StabilizationSetpoint guidance_h_guided_run ( bool in_flight)

Run GUIDED mode control.

Definition at line 457 of file guidance_h.c.

References guidance_h, guidance_h_cmd, guidance_h_guided_run(), guidance_h_hover_enter(), guidance_h_run_pos(), guidance_h_run_speed(), and guidance_h_update_reference().

Referenced by guidance_h_guided_run(), and guidance_h_run().

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

◆ guidance_h_hover_enter()

void guidance_h_hover_enter ( void )

◆ guidance_h_init()

void guidance_h_init ( void )

◆ guidance_h_mode_changed()

void guidance_h_mode_changed ( uint8_t new_mode)

◆ guidance_h_nav_enter()

void guidance_h_nav_enter ( void )

Definition at line 380 of file guidance_h.c.

References guidance_h_run_enter(), guidance_h_set_heading(), guidance_h_set_pos(), nav, and reset_guidance_reference_from_current_position().

Referenced by guidance_h_from_nav(), and guidance_h_mode_changed().

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

◆ guidance_h_run()

struct StabilizationSetpoint guidance_h_run ( bool in_flight)

◆ guidance_h_set_acc()

void guidance_h_set_acc ( float ax,
float ay )

Set horizontal acceleration setpoint.

Parameters
axNorth acceleration (local NED frame) in meters/sec².
ayEast acceleration (local NED frame) in meters/sec².

Definition at line 520 of file guidance_h.c.

References ACCEL_BFP_OF_REAL, guidance_h, and reset_guidance_reference_from_current_position().

Referenced by guidance_h_set_body_acc().

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

◆ guidance_h_set_all()

void guidance_h_set_all ( float x,
float y,
float vx,
float vy,
float ax,
float ay )

Set position speed and acceleration (NED frame).

Parameters
xNorth position (local NED frame) in meters.
yEast position (local NED frame) in meters.
vxNorth velocity (local NED frame) in meters/sec.
vyEast velocity (local NED frame) in meters/sec.
axNorth acceleration (local NED frame) in meters/sec².
ayEast acceleration (local NED frame) in meters/sec².

Definition at line 536 of file guidance_h.c.

References ACCEL_BFP_OF_REAL, guidance_h, POS_BFP_OF_REAL, and SPEED_BFP_OF_REAL.

◆ guidance_h_set_body_acc()

void guidance_h_set_body_acc ( float ax,
float ay )

Set body relative horizontal acceleration setpoint.

Parameters
axforward acceleration (body frame) in meters/sec².
ayright acceleration (body frame) in meters/sec².

Definition at line 512 of file guidance_h.c.

References guidance_h_set_acc(), FloatEulers::psi, and stateGetNedToBodyEulers_f().

Here is the call graph for this function:

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

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

Referenced by 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)

Set heading setpoint.

Parameters
headingSetpoint in radians.

Definition at line 487 of file guidance_h.c.

References FLOAT_ANGLE_NORMALIZE, guidance_h, and heading.

Referenced by guidance_h_hover_enter(), guidance_h_nav_enter(), orange_avoider_guided_periodic(), and wedgebug_periodic().

Here is the caller graph for this function:

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

References guidance_h.

Referenced by orange_avoider_guided_periodic().

Here is the caller graph for this function:

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

References guidance_h, POS_BFP_OF_REAL, and reset_guidance_reference_from_current_position().

Referenced by guidance_h_hover_enter(), guidance_h_nav_enter(), and wedgebug_periodic().

Here is the call graph for this function:
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 502 of file guidance_h.c.

References guidance_h, reset_guidance_reference_from_current_position(), and SPEED_BFP_OF_REAL.

Referenced by guidance_h_set_body_vel().

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

◆ guidance_h_update_reference()

◆ rc_cb()

void rc_cb ( uint8_t sender_id UNUSED,
struct RadioControl * rc )
static

◆ read_rc_setpoint_heading()

void read_rc_setpoint_heading ( struct HorizontalGuidanceRCInput * rc_sp,
bool in_flight,
struct RadioControl * rc )
static

◆ read_rc_setpoint_speed_i()

void read_rc_setpoint_speed_i ( struct Int32Vect2 * speed_sp,
bool in_flight,
struct RadioControl * rc )
static

read speed setpoint from RC

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

Definition at line 189 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_PITCH, RADIO_ROLL, SPEED_BFP_OF_REAL, speed_sp, stateGetNedToBodyEulers_i(), and RadioControl::values.

Referenced by rc_cb().

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

◆ reset_guidance_reference_from_current_position()

void reset_guidance_reference_from_current_position ( void )
inlinestatic

Definition at line 114 of file guidance_h.c.

References FLOAT_VECT2_ZERO, gh_set_ref(), guidance_h, INT_VECT2_ZERO, SPEED_FLOAT_OF_BFP, stateGetPositionNed_i(), stateGetSpeedNed_i(), VECT2_COPY, FloatVect2::x, and FloatVect2::y.

Referenced by guidance_h_hover_enter(), guidance_h_nav_enter(), guidance_h_set_acc(), guidance_h_set_pos(), and guidance_h_set_vel().

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

◆ send_gh()

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

Definition at line 64 of file guidance_h.c.

References dev, guidance_h, stateGetPositionNed_i(), NedCoor_i::x, and NedCoor_i::y.

Referenced by guidance_h_init().

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

◆ send_href()

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

Definition at line 73 of file guidance_h.c.

References dev, and guidance_h.

Referenced by guidance_h_init().

Here is the caller graph for this function:

Variable Documentation

◆ guidance_h

◆ guidance_h_cmd

struct StabilizationSetpoint guidance_h_cmd

horizontal guidance command.

In north/east with INT32_ANGLE_FRAC

Definition at line 50 of file guidance_h.c.

Referenced by guidance_h_guided_run().

◆ rc_ev

abi_event rc_ev
static

Definition at line 58 of file guidance_h.c.