|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Horizontal guidance for rotorcrafts. More...
#include "generated/airframe.h"#include "firmwares/rotorcraft/guidance/guidance_h.h"#include "firmwares/rotorcraft/guidance/guidance_module.h"#include "firmwares/rotorcraft/stabilization.h"#include "firmwares/rotorcraft/navigation.h"#include "firmwares/rotorcraft/autopilot_rc_helpers.h"#include "modules/radio_control/radio_control.h"#include "modules/core/abi.h"#include "firmwares/rotorcraft/guidance/guidance_v.h"#include "state.h"#include "modules/datalink/telemetry.h"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 |
Horizontal guidance for rotorcrafts.
Definition in file guidance_h.c.
| #define GUIDANCE_H_DEADBAND_R 200 |
Definition at line 160 of file guidance_h.c.
| #define GUIDANCE_H_RC_ID ABI_BROADCAST |
Definition at line 55 of file guidance_h.c.
Referenced by guidance_h_init().
| #define GUIDANCE_H_SP_MAX_R 60.f |
Definition at line 155 of file guidance_h.c.
Referenced by read_rc_setpoint_heading().
| #define YAW_DEADBAND_EXCEEDED | ( | _rc | ) |
Definition at line 163 of file guidance_h.c.
Referenced by read_rc_setpoint_heading().
| 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().
| 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().
| void guidance_h_hover_enter | ( | void | ) |
Definition at line 359 of file guidance_h.c.
References guidance_h, guidance_h_run_enter(), guidance_h_set_heading(), guidance_h_set_pos(), FloatEulers::psi, reset_guidance_reference_from_current_position(), stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), FloatVect2::x, and FloatVect2::y.
Referenced by guidance_h_guided_run(), guidance_h_mode_changed(), and wedgebug_periodic().
| void guidance_h_init | ( | void | ) |
Definition at line 86 of file guidance_h.c.
References DefaultPeriodic, gh_ref_init(), guidance_h, GUIDANCE_H_MODE_NONE, GUIDANCE_H_RC_ID, GUIDANCE_H_USE_REF, INT_VECT2_ZERO, rc_cb(), rc_ev, register_periodic_telemetry(), send_gh(), and send_href().
| void guidance_h_mode_changed | ( | uint8_t | new_mode | ) |
Definition at line 128 of file guidance_h.c.
References guidance_h, guidance_h_hover_enter(), GUIDANCE_H_MODE_GUIDED, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, and guidance_h_nav_enter().
Referenced by autopilot_static_set_mode(), control_mixing_heewing_attitude_direct_enter(), control_mixing_heewing_attitude_plane_enter(), control_mixing_heewing_nav_enter(), and guidance_module_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().
| struct StabilizationSetpoint guidance_h_run | ( | bool | in_flight | ) |
Definition at line 244 of file guidance_h.c.
References guidance_h, guidance_h_from_nav(), guidance_h_guided_run(), GUIDANCE_H_MODE_GUIDED, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, guidance_h_run(), StabilizationSetpoint::sp, and STAB_SP_SET_EULERS_ZERO.
Referenced by autopilot_static_periodic(), control_mixing_heewing_nav_run(), guidance_h_run(), and guidance_module_run().
| void guidance_h_set_acc | ( | float | ax, |
| float | ay ) |
Set horizontal acceleration setpoint.
| ax | North acceleration (local NED frame) in meters/sec². |
| ay | East 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().
| void guidance_h_set_all | ( | float | x, |
| float | y, | ||
| float | vx, | ||
| float | vy, | ||
| float | ax, | ||
| float | ay ) |
Set position speed and acceleration (NED frame).
| x | North position (local NED frame) in meters. |
| y | East position (local NED frame) in meters. |
| vx | North velocity (local NED frame) in meters/sec. |
| vy | East velocity (local NED frame) in meters/sec. |
| ax | North acceleration (local NED frame) in meters/sec². |
| ay | East 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.
| void guidance_h_set_body_acc | ( | float | ax, |
| float | ay ) |
Set body relative horizontal acceleration setpoint.
| ax | forward acceleration (body frame) in meters/sec². |
| ay | right 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().
| void guidance_h_set_body_vel | ( | float | vx, |
| float | vy ) |
Set body relative horizontal velocity setpoint.
| vx | forward velocity (body frame) in meters/sec. |
| vy | right 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().
| void guidance_h_set_heading | ( | float | heading | ) |
Set heading setpoint.
| heading | Setpoint 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().
| void guidance_h_set_heading_rate | ( | float | rate | ) |
Set heading rate setpoint.
| rate | Heading rate in radians. |
Definition at line 530 of file guidance_h.c.
References guidance_h.
Referenced by orange_avoider_guided_periodic().
| void guidance_h_set_pos | ( | float | x, |
| float | y ) |
Set horizontal position setpoint.
| x | North position (local NED frame) in meters. |
| y | East 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().
| void guidance_h_set_vel | ( | float | vx, |
| float | vy ) |
Set horizontal velocity setpoint.
| vx | North velocity (local NED frame) in meters/sec. |
| vy | East 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().
|
static |
Definition at line 273 of file guidance_h.c.
References ACCEL_BFP_OF_REAL, ACCEL_FLOAT_OF_BFP, FLOAT_ANGLE_NORMALIZE, GH_POS_REF_FRAC, gh_ref, gh_set_ref(), gh_update_ref_from_accel_sp(), gh_update_ref_from_pos_sp(), gh_update_ref_from_speed_sp(), guidance_h, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, INT32_POS_FRAC, INT32_VECT2_RSHIFT, INT_VECT2_ZERO, nav, NAV_SETPOINT_MODE_ACCEL, NAV_SETPOINT_MODE_POS, NAV_SETPOINT_MODE_SPEED, sp_accel, SPEED_BFP_OF_REAL, SPEED_FLOAT_OF_BFP, stateGetPositionNed_i(), stateGetSpeedNed_i(), VECT2_COPY, FloatVect2::x, NedCoor_i::x, FloatVect2::y, and NedCoor_i::y.
Referenced by guidance_h_guided_run().
|
static |
Definition at line 219 of file guidance_h.c.
References autopilot_in_flight(), guidance_h, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, INT_VECT2_ZERO, radio_control, RC_OK, read_rc_setpoint_heading(), read_rc_setpoint_speed_i(), and UNUSED.
|
static |
Definition at line 167 of file guidance_h.c.
References FLOAT_ANGLE_NORMALIZE, get_sys_time_float(), GUIDANCE_H_SP_MAX_R, HorizontalGuidanceRCInput::heading, HorizontalGuidanceRCInput::last_ts, MAX_PPRZ, FloatEulers::psi, RADIO_YAW, stateGetNedToBodyEulers_f(), THROTTLE_STICK_DOWN_FROM_RC, RadioControl::values, and YAW_DEADBAND_EXCEEDED.
Referenced by rc_cb().
|
static |
read speed setpoint from RC
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().
|
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().
|
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().
|
static |
Definition at line 73 of file guidance_h.c.
References dev, and guidance_h.
Referenced by guidance_h_init().
| struct HorizontalGuidance guidance_h |
Definition at line 45 of file guidance_h.c.
Referenced by autopilot_static_periodic(), guidance_h_guided_run(), guidance_h_hover_enter(), guidance_h_init(), guidance_h_mode_changed(), guidance_h_run(), guidance_h_set_acc(), guidance_h_set_all(), guidance_h_set_heading(), guidance_h_set_heading_rate(), guidance_h_set_pos(), guidance_h_set_vel(), guidance_h_update_reference(), guidance_pid_h_run(), ins_ekf2_publish_attitude(), orange_avoider_guided_periodic(), rc_cb(), reset_guidance_reference_from_current_position(), rotwing_state_periodic(), send_fp(), send_gh(), send_hover_loop(), send_href(), send_status(), and stabilization_indi_set_wls_settings().
| 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().
|
static |
Definition at line 58 of file guidance_h.c.