Paparazzi UAS
v7.0_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 More... | |
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. More... | |
struct StabilizationSetpoint | guidance_h_guided_run (bool in_flight) |
Run GUIDED mode control. More... | |
void | guidance_h_set_pos (float x, float y) |
Set horizontal position setpoint. More... | |
void | guidance_h_set_heading (float heading) |
Set heading setpoint. More... | |
void | guidance_h_set_body_vel (float vx, float vy) |
Set body relative horizontal velocity setpoint. More... | |
void | guidance_h_set_vel (float vx, float vy) |
Set horizontal acceleration setpoint. More... | |
void | guidance_h_set_body_acc (float ax, float ay) |
Set body relative horizontal acceleration setpoint. More... | |
void | guidance_h_set_acc (float ax, float ay) |
Set horizontal velocity setpoint. More... | |
void | guidance_h_set_heading_rate (float rate) |
Set heading rate setpoint. More... | |
Variables | |
struct HorizontalGuidance | guidance_h |
struct StabilizationSetpoint | guidance_h_cmd |
horizontal guidance command. More... | |
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.
#define GUIDANCE_H_SP_MAX_R 60.f |
Definition at line 155 of file guidance_h.c.
#define YAW_DEADBAND_EXCEEDED | ( | _rc | ) |
Definition at line 163 of file guidance_h.c.
struct StabilizationSetpoint guidance_h_from_nav | ( | bool | in_flight | ) |
Set horizontal guidance from NAV and run control loop.
Definition at line 364 of file guidance_h.c.
References RotorcraftNavigation::carrot, guidance_h_run_enter(), guidance_h_set_heading(), guidance_h_set_pos(), RotorcraftNavigation::heading, nav, reset_guidance_reference_from_current_position(), EnuCoor_f::x, and EnuCoor_f::y.
struct StabilizationSetpoint guidance_h_guided_run | ( | bool | in_flight | ) |
Run GUIDED mode control.
Definition at line 364 of file guidance_h.c.
void guidance_h_hover_enter | ( | void | ) |
Definition at line 343 of file guidance_h.c.
References guidance_h, guidance_h_run_enter(), guidance_h_set_heading(), guidance_h_set_pos(), HorizontalGuidanceRCInput::heading, FloatEulers::psi, HorizontalGuidance::rc_sp, reset_guidance_reference_from_current_position(), HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), FloatVect2::x, Int32Vect2::x, FloatVect2::y, and Int32Vect2::y.
Referenced by guidance_h_mode_changed().
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, HorizontalGuidanceSetpoint::h_mask, HorizontalGuidanceSetpoint::heading, HorizontalGuidanceRCInput::heading, HorizontalGuidanceSetpoint::heading_rate, INT_VECT2_ZERO, HorizontalGuidanceRCInput::last_ts, HorizontalGuidance::mode, HorizontalGuidanceSetpoint::pos, rc_cb(), rc_ev, HorizontalGuidance::rc_sp, register_periodic_telemetry(), send_gh(), send_href(), HorizontalGuidance::sp, HorizontalGuidance::use_ref, HorizontalGuidanceRCInput::vect, and HorizontalGuidanceSetpoint::yaw_mask.
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, guidance_h_nav_enter(), and HorizontalGuidance::mode.
Referenced by autopilot_static_set_mode(), and guidance_module_enter().
void guidance_h_nav_enter | ( | void | ) |
Definition at line 364 of file guidance_h.c.
Referenced by guidance_h_mode_changed().
struct StabilizationSetpoint guidance_h_run | ( | bool | in_flight | ) |
Definition at line 219 of file guidance_h.c.
References autopilot_in_flight(), guidance_h, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, HorizontalGuidanceSetpoint::h_mask, INT_VECT2_ZERO, HorizontalGuidance::mode, radio_control, RC_OK, HorizontalGuidance::rc_sp, read_rc_setpoint_heading(), read_rc_setpoint_speed_i(), HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, RadioControl::status, and HorizontalGuidanceRCInput::vect.
Referenced by autopilot_static_periodic(), and guidance_module_run().
void guidance_h_set_acc | ( | float | ax, |
float | ay | ||
) |
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 498 of file guidance_h.c.
References HorizontalGuidanceSetpoint::accel, ACCEL_BFP_OF_REAL, guidance_h, HorizontalGuidanceSetpoint::h_mask, reset_guidance_reference_from_current_position(), HorizontalGuidance::sp, Int32Vect2::x, and Int32Vect2::y.
Referenced by guidance_h_set_body_acc().
void guidance_h_set_body_acc | ( | float | ax, |
float | ay | ||
) |
Set body relative horizontal acceleration setpoint.
vx | forward acceleration (body frame) in meters/secĀ². |
vy | right acceleration (body frame) in meters/secĀ². |
Definition at line 490 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 472 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 465 of file guidance_h.c.
References FLOAT_ANGLE_NORMALIZE, guidance_h, HorizontalGuidanceSetpoint::heading, heading, HorizontalGuidance::sp, and HorizontalGuidanceSetpoint::yaw_mask.
Referenced by guidance_h_from_nav(), guidance_h_hover_enter(), and orange_avoider_guided_periodic().
void guidance_h_set_heading_rate | ( | float | rate | ) |
Set heading rate setpoint.
rate | Heading rate in radians. |
Definition at line 508 of file guidance_h.c.
References guidance_h, HorizontalGuidanceSetpoint::heading_rate, HorizontalGuidance::sp, and HorizontalGuidanceSetpoint::yaw_mask.
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 455 of file guidance_h.c.
References guidance_h, HorizontalGuidanceSetpoint::h_mask, HorizontalGuidanceSetpoint::pos, POS_BFP_OF_REAL, reset_guidance_reference_from_current_position(), HorizontalGuidance::sp, Int32Vect2::x, and Int32Vect2::y.
Referenced by guidance_h_from_nav(), and guidance_h_hover_enter().
void guidance_h_set_vel | ( | float | vx, |
float | vy | ||
) |
Set horizontal acceleration setpoint.
vx | North acceleration (local NED frame) in meters/secĀ². |
vy | East acceleration (local NED frame) in meters/secĀ². |
Definition at line 480 of file guidance_h.c.
References guidance_h, HorizontalGuidanceSetpoint::h_mask, reset_guidance_reference_from_current_position(), HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, SPEED_BFP_OF_REAL, Int32Vect2::x, and Int32Vect2::y.
Referenced by guidance_h_set_body_vel().
|
static |
Definition at line 273 of file guidance_h.c.
References HorizontalGuidanceReference::accel, HorizontalGuidanceSetpoint::accel, GuidanceHRef::accel, ACCEL_BFP_OF_REAL, ACCEL_FLOAT_OF_BFP, FLOAT_ANGLE_NORMALIZE, GH_POS_REF_FRAC, gh_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, HorizontalGuidanceSetpoint::h_mask, HorizontalGuidanceSetpoint::heading, HorizontalGuidanceSetpoint::heading_rate, INT32_POS_FRAC, INT32_VECT2_RSHIFT, INT_VECT2_ZERO, HorizontalGuidance::mode, nav, NAV_SETPOINT_MODE_ACCEL, NAV_SETPOINT_MODE_POS, NAV_SETPOINT_MODE_SPEED, HorizontalGuidanceReference::pos, HorizontalGuidanceSetpoint::pos, GuidanceHRef::pos, HorizontalGuidance::ref, RotorcraftNavigation::setpoint_mode, HorizontalGuidance::sp, HorizontalGuidanceReference::speed, HorizontalGuidanceSetpoint::speed, GuidanceHRef::speed, SPEED_BFP_OF_REAL, SPEED_FLOAT_OF_BFP, stateGetPositionNed_i(), stateGetSpeedNed_i(), HorizontalGuidance::use_ref, VECT2_COPY, FloatVect2::x, Int32Vect2::x, NedCoor_i::x, FloatVect2::y, Int32Vect2::y, NedCoor_i::y, and HorizontalGuidanceSetpoint::yaw_mask.
|
static |
Definition at line 219 of file guidance_h.c.
Referenced by guidance_h_init().
|
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 guidance_h_run().
|
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(), RadioControl::values, FloatVect3::x, and FloatVect3::y.
Referenced by guidance_h_run().
|
inlinestatic |
Definition at line 114 of file guidance_h.c.
References HorizontalGuidanceReference::accel, FLOAT_VECT2_ZERO, gh_set_ref(), guidance_h, INT_VECT2_ZERO, HorizontalGuidanceReference::pos, HorizontalGuidance::ref, HorizontalGuidanceReference::speed, SPEED_FLOAT_OF_BFP, stateGetPositionNed_i(), stateGetSpeedNed_i(), VECT2_COPY, FloatVect2::x, Int32Vect2::x, FloatVect2::y, and Int32Vect2::y.
Referenced by guidance_h_from_nav(), guidance_h_hover_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, HorizontalGuidanceReference::pos, HorizontalGuidanceSetpoint::pos, HorizontalGuidance::ref, HorizontalGuidance::sp, stateGetPositionNed_i(), Int32Vect2::x, NedCoor_i::x, Int32Vect2::y, and NedCoor_i::y.
Referenced by guidance_h_init().
|
static |
Definition at line 73 of file guidance_h.c.
References HorizontalGuidanceReference::accel, dev, guidance_h, HorizontalGuidanceReference::pos, HorizontalGuidanceSetpoint::pos, HorizontalGuidance::ref, HorizontalGuidance::sp, HorizontalGuidanceReference::speed, HorizontalGuidanceSetpoint::speed, Int32Vect2::x, and Int32Vect2::y.
Referenced by guidance_h_init().
struct HorizontalGuidance guidance_h |
Definition at line 1 of file guidance_h.c.
Referenced by autopilot_static_periodic(), guidance_h_hover_enter(), guidance_h_init(), guidance_h_mode_changed(), guidance_h_run(), guidance_h_set_acc(), guidance_h_set_heading(), guidance_h_set_heading_rate(), guidance_h_set_pos(), guidance_h_set_vel(), guidance_h_update_reference(), ins_ekf2_publish_attitude(), orange_avoider_guided_periodic(), periodic_rotwing_state(), 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 1 of file guidance_h.c.
|
static |
Definition at line 58 of file guidance_h.c.
Referenced by guidance_h_init().