Paparazzi UAS
v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
|
Horizontal guidance for rotorcrafts. More...
#include "math/pprz_algebra_int.h"
#include "firmwares/rotorcraft/guidance/guidance_h_ref.h"
#include "generated/airframe.h"
#include "std.h"
Go to the source code of this file.
Data Structures | |
struct | HorizontalGuidanceSetpoint |
struct | HorizontalGuidanceReference |
struct | HorizontalGuidanceGains |
struct | HorizontalGuidance |
Macros | |
#define | GUIDANCE_H_USE_REF TRUE |
Use horizontal guidance reference trajectory. More... | |
#define | GUIDANCE_H_USE_SPEED_REF TRUE |
Use horizontal guidance speed reference. More... | |
#define | GUIDANCE_H_MODE_KILL 0 |
#define | GUIDANCE_H_MODE_RATE 1 |
#define | GUIDANCE_H_MODE_ATTITUDE 2 |
#define | GUIDANCE_H_MODE_HOVER 3 |
#define | GUIDANCE_H_MODE_NAV 4 |
#define | GUIDANCE_H_MODE_RC_DIRECT 5 |
#define | GUIDANCE_H_MODE_CARE_FREE 6 |
#define | GUIDANCE_H_MODE_FORWARD 7 |
#define | GUIDANCE_H_MODE_MODULE 8 |
#define | GUIDANCE_H_MODE_FLIP 9 |
#define | GUIDANCE_H_MODE_GUIDED 10 |
#define | guidance_h_SetUseRef(_val) |
Functions | |
void | guidance_h_init (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_set_igain (uint32_t igain) |
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 Int32Vect2 * | guidance_h_get_pos_err (void) |
Gets the position error. More... | |
static void | guidance_h_SetMaxSpeed (float speed) |
static void | guidance_h_SetOmega (float omega) |
static void | guidance_h_SetZeta (float zeta) |
static void | guidance_h_SetTau (float tau) |
Variables | |
struct HorizontalGuidance | guidance_h |
int32_t | transition_percentage |
int32_t | transition_theta_offset |
Horizontal guidance for rotorcrafts.
Definition in file guidance_h.h.
struct HorizontalGuidanceSetpoint |
Definition at line 65 of file guidance_h.h.
Data Fields | ||
---|---|---|
int32_t | heading | with INT32_ANGLE_FRAC |
int32_t | heading_rate | with INT32_RATE_FRAC |
uint8_t | mask | bit 5: vx & vy, bit 6: vz, bit 7: vyaw |
struct Int32Vect2 | pos |
horizontal position setpoint in NED. fixed point representation: Q23.8 accuracy 0.0039, range 8388km |
struct Int32Vect2 | speed | only used in HOVER mode if GUIDANCE_H_USE_SPEED_REF or in GUIDED mode |
struct HorizontalGuidanceReference |
Definition at line 77 of file guidance_h.h.
Data Fields | ||
---|---|---|
struct Int32Vect2 | accel | with INT32_ACCEL_FRAC |
struct Int32Vect2 | pos | with INT32_POS_FRAC |
struct Int32Vect2 | speed | with INT32_SPEED_FRAC |
struct HorizontalGuidanceGains |
struct HorizontalGuidance |
Definition at line 91 of file guidance_h.h.
Data Fields | ||
---|---|---|
bool | approx_force_by_thrust | |
struct HorizontalGuidanceGains | gains | |
uint8_t | mode | |
struct Int32Eulers | rc_sp | with INT32_ANGLE_FRAC |
struct HorizontalGuidanceReference | ref | reference calculated from setpoints |
struct HorizontalGuidanceSetpoint | sp | setpoints |
bool | use_ref |
#define GUIDANCE_H_MODE_ATTITUDE 2 |
Definition at line 54 of file guidance_h.h.
Referenced by autopilot_set_mode(), booz_fms_impl_init(), guidance_h_mode_changed(), guidance_h_read_rc(), guidance_h_run(), vi_init(), and vi_periodic().
#define GUIDANCE_H_MODE_CARE_FREE 6 |
Definition at line 58 of file guidance_h.h.
Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().
#define GUIDANCE_H_MODE_FLIP 9 |
Definition at line 61 of file guidance_h.h.
Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().
#define GUIDANCE_H_MODE_FORWARD 7 |
Definition at line 59 of file guidance_h.h.
Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().
#define GUIDANCE_H_MODE_GUIDED 10 |
Definition at line 62 of file guidance_h.h.
Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_run(), guidance_h_set_guided_heading(), guidance_h_set_guided_heading_rate(), guidance_h_set_guided_pos(), and guidance_h_set_guided_vel().
#define GUIDANCE_H_MODE_HOVER 3 |
Definition at line 55 of file guidance_h.h.
Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), guidance_h_run(), and guidance_h_update_reference().
#define GUIDANCE_H_MODE_KILL 0 |
Definition at line 52 of file guidance_h.h.
Referenced by autopilot_set_mode(), guidance_h_init(), and guidance_h_mode_changed().
#define GUIDANCE_H_MODE_MODULE 8 |
Definition at line 60 of file guidance_h.h.
Referenced by guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().
#define GUIDANCE_H_MODE_NAV 4 |
Definition at line 56 of file guidance_h.h.
Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().
#define GUIDANCE_H_MODE_RATE 1 |
Definition at line 53 of file guidance_h.h.
Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().
#define GUIDANCE_H_MODE_RC_DIRECT 5 |
Definition at line 57 of file guidance_h.h.
Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().
#define guidance_h_SetUseRef | ( | _val | ) |
Definition at line 159 of file guidance_h.h.
#define GUIDANCE_H_USE_REF TRUE |
Use horizontal guidance reference trajectory.
Default is TRUE, define to FALSE to always disable it.
Definition at line 41 of file guidance_h.h.
Referenced by guidance_h_init().
#define GUIDANCE_H_USE_SPEED_REF TRUE |
Use horizontal guidance speed reference.
This also allows to give velocity commands via RC in GUIDANCE_H_MODE_HOVER. Default is TRUE, define to FALSE to always disable it.
Definition at line 49 of file guidance_h.h.
const struct Int32Vect2* guidance_h_get_pos_err | ( | void | ) |
Gets the position error.
none. |
Definition at line 732 of file guidance_h.c.
References guidance_h_pos_err.
void guidance_h_init | ( | void | ) |
Definition at line 169 of file guidance_h.c.
References HorizontalGuidanceGains::a, HorizontalGuidance::approx_force_by_thrust, HorizontalGuidanceGains::d, DefaultPeriodic, HorizontalGuidance::gains, gh_ref_init(), guidance_h, GUIDANCE_H_AGAIN, GUIDANCE_H_APPROX_FORCE_BY_THRUST, GUIDANCE_H_MODE_KILL, guidance_h_module_init(), guidance_h_trim_att_integrator, GUIDANCE_H_USE_REF, GUIDANCE_H_VGAIN, guidance_hybrid_init(), guidance_indi_enter(), HorizontalGuidanceSetpoint::heading, HorizontalGuidanceSetpoint::heading_rate, HorizontalGuidanceGains::i, INT_EULERS_ZERO, INT_VECT2_ZERO, HorizontalGuidance::mode, HorizontalGuidanceGains::p, HorizontalGuidanceSetpoint::pos, HorizontalGuidance::rc_sp, register_periodic_telemetry(), send_gh(), send_hover_loop(), send_href(), send_tune_hover(), HorizontalGuidance::sp, HorizontalGuidance::use_ref, and HorizontalGuidanceGains::v.
Referenced by autopilot_init().
void guidance_h_mode_changed | ( | uint8_t | new_mode | ) |
Definition at line 222 of file guidance_h.c.
References guidance_flip_enter(), guidance_h, guidance_h_hover_enter(), GUIDANCE_H_MODE_ATTITUDE, GUIDANCE_H_MODE_CARE_FREE, GUIDANCE_H_MODE_FLIP, GUIDANCE_H_MODE_FORWARD, GUIDANCE_H_MODE_GUIDED, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_KILL, GUIDANCE_H_MODE_MODULE, GUIDANCE_H_MODE_NAV, GUIDANCE_H_MODE_RATE, GUIDANCE_H_MODE_RC_DIRECT, guidance_h_module_enter(), guidance_h_nav_enter(), guidance_hybrid_norm_ref_airspeed, guidance_indi_enter(), HorizontalGuidance::mode, stabilization_attitude_enter(), stabilization_attitude_reset_care_free_heading(), stabilization_none_enter(), and stabilization_rate_enter().
Referenced by autopilot_set_mode().
void guidance_h_read_rc | ( | bool | in_flight | ) |
Definition at line 306 of file guidance_h.c.
References FALSE, guidance_h, GUIDANCE_H_MODE_ATTITUDE, GUIDANCE_H_MODE_CARE_FREE, GUIDANCE_H_MODE_FLIP, GUIDANCE_H_MODE_FORWARD, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_MODULE, GUIDANCE_H_MODE_NAV, GUIDANCE_H_MODE_RATE, GUIDANCE_H_MODE_RC_DIRECT, guidance_h_module_read_rc(), INT_EULERS_ZERO, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, radio_control, RC_OK, HorizontalGuidance::rc_sp, read_rc_setpoint_speed_i(), HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, stabilization_attitude_read_rc(), stabilization_attitude_read_rc_setpoint_eulers(), stabilization_none_read_rc(), stabilization_rate_read_rc(), stabilization_rate_read_rc_switched_sticks(), RadioControl::status, and TRUE.
Referenced by autopilot_on_rc_frame().
void guidance_h_run | ( | bool | in_flight | ) |
Definition at line 365 of file guidance_h.c.
References guidance_flip_run(), guidance_h, guidance_h_cmd_earth, guidance_h_hover_enter(), GUIDANCE_H_MODE_ATTITUDE, GUIDANCE_H_MODE_CARE_FREE, GUIDANCE_H_MODE_FLIP, GUIDANCE_H_MODE_FORWARD, GUIDANCE_H_MODE_GUIDED, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_MODULE, GUIDANCE_H_MODE_NAV, GUIDANCE_H_MODE_RATE, GUIDANCE_H_MODE_RC_DIRECT, guidance_h_module_run(), guidance_h_nav_enter(), guidance_h_traj_run(), guidance_h_update_reference(), guidance_hybrid_reset_heading(), guidance_hybrid_run(), guidance_indi_run(), HorizontalGuidanceSetpoint::heading, horizontal_mode, HORIZONTAL_MODE_ATTITUDE, HORIZONTAL_MODE_MANUAL, INT32_ANGLE_NORMALIZE, INT32_PERCENTAGE_FRAC, INT32_VECT2_NED_OF_ENU, HorizontalGuidance::mode, nav_cmd_pitch, nav_cmd_roll, nav_cmd_yaw, nav_heading, nav_pitch, nav_roll, navigation_carrot, navigation_target, Int32Eulers::phi, HorizontalGuidanceSetpoint::pos, Int32Eulers::psi, HorizontalGuidance::rc_sp, HorizontalGuidance::sp, stabilization_attitude_run(), stabilization_attitude_set_earth_cmd_i(), stabilization_attitude_set_rpy_setpoint_i(), stabilization_cmd, stabilization_none_run(), stabilization_rate_run(), Int32Eulers::theta, and transition_run().
Referenced by autopilot_periodic().
bool guidance_h_set_guided_body_vel | ( | float | vx, |
float | vy | ||
) |
Set body relative horizontal velocity setpoint in GUIDED mode.
vx | forward velocity (body frame) in meters/sec. |
vy | right velocity (body frame) in meters/sec. |
Definition at line 703 of file guidance_h.c.
References guidance_h_set_guided_vel(), FloatEulers::psi, Int32Eulers::psi, and stateGetNedToBodyEulers_f().
Referenced by autopilot_guided_update().
bool guidance_h_set_guided_heading | ( | float | heading | ) |
Set heading setpoint in GUIDED mode.
heading | Setpoint in radians. |
Definition at line 692 of file guidance_h.c.
References ANGLE_BFP_OF_REAL, guidance_h, GUIDANCE_H_MODE_GUIDED, HorizontalGuidanceSetpoint::heading, INT32_ANGLE_NORMALIZE, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, and HorizontalGuidance::sp.
Referenced by autopilot_guided_goto_ned(), autopilot_guided_move_ned(), and autopilot_guided_update().
bool guidance_h_set_guided_heading_rate | ( | float | rate | ) |
Set heading rate setpoint in GUIDED mode.
rate | Heading rate in radians. |
Definition at line 722 of file guidance_h.c.
References guidance_h, GUIDANCE_H_MODE_GUIDED, HorizontalGuidanceSetpoint::heading_rate, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, RATE_BFP_OF_REAL, and HorizontalGuidance::sp.
Referenced by autopilot_guided_update().
bool guidance_h_set_guided_pos | ( | float | x, |
float | y | ||
) |
Set horizontal position setpoint in GUIDED mode.
x | North position (local NED frame) in meters. |
y | East position (local NED frame) in meters. |
Definition at line 681 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().
bool guidance_h_set_guided_vel | ( | float | vx, |
float | vy | ||
) |
Set horizontal velocity setpoint in GUIDED mode.
vx | North velocity (local NED frame) in meters/sec. |
vy | East velocity (local NED frame) in meters/sec. |
Definition at line 711 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().
void guidance_h_set_igain | ( | uint32_t | igain | ) |
Definition at line 675 of file guidance_h.c.
References HorizontalGuidance::gains, guidance_h, guidance_h_trim_att_integrator, HorizontalGuidanceGains::i, and INT_VECT2_ZERO.
|
inlinestatic |
Definition at line 163 of file guidance_h.h.
References gh_set_max_speed().
|
inlinestatic |
Definition at line 168 of file guidance_h.h.
References gh_set_omega().
|
inlinestatic |
Definition at line 178 of file guidance_h.h.
References gh_set_tau().
|
inlinestatic |
Definition at line 173 of file guidance_h.h.
References gh_set_zeta().
struct HorizontalGuidance guidance_h |
Definition at line 80 of file guidance_h.c.
Referenced by gps_sim_hitl_event(), guidance_h_hover_enter(), guidance_h_init(), guidance_h_mode_changed(), guidance_h_nav_enter(), guidance_h_read_rc(), guidance_h_run(), guidance_h_set_guided_heading(), guidance_h_set_guided_heading_rate(), guidance_h_set_guided_pos(), guidance_h_set_guided_vel(), guidance_h_set_igain(), guidance_h_traj_run(), guidance_h_update_reference(), guidance_hybrid_position_to_airspeed(), guidance_indi_run(), reset_guidance_reference_from_current_position(), send_fp(), send_gh(), send_hover_loop(), send_href(), and send_status().
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().