![]() |
Paparazzi UAS
v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Vertical guidance for rotorcrafts. More...
#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
#include "modules/radio_control/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/navigation.h"
#include "state.h"
#include "math/pprz_algebra_int.h"
#include "modules/datalink/telemetry.h"
Go to the source code of this file.
Macros | |
#define | GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4 |
#define | GUIDANCE_V_CLIMB_RC_DEADBAND MAX_PPRZ/10 |
#define | GUIDANCE_V_MAX_RC_CLIMB_SPEED GUIDANCE_V_REF_MIN_ZD |
#define | GUIDANCE_V_MAX_RC_DESCENT_SPEED GUIDANCE_V_REF_MAX_ZD |
#define | GUIDANCE_V_GUIDED_MODE_ZHOLD 0 |
#define | GUIDANCE_V_GUIDED_MODE_CLIMB 1 |
#define | GUIDANCE_V_GUIDED_MODE_THROTTLE 2 |
Functions | |
static void | send_tune_vert (struct transport_tx *trans, struct link_device *dev) |
void | guidance_v_init (void) |
void | guidance_v_read_rc (void) |
void | guidance_v_mode_changed (uint8_t new_mode) |
void | guidance_v_notify_in_flight (bool in_flight) |
static int32_t | get_vertical_thrust_coeff (void) |
get the cosine of the angle between thrust vector and gravity vector More... | |
void | guidance_v_thrust_adapt (bool in_flight) |
void | guidance_v_run (bool in_flight) |
void | guidance_v_z_enter (void) |
void | guidance_v_set_ref (int32_t pos, int32_t speed, int32_t accel) |
Set guidance ref parameters. More... | |
void | guidance_v_update_ref (void) |
void | guidance_v_from_nav (bool in_flight) |
Set guidance setpoint from NAV and run hover loop. More... | |
void | guidance_v_guided_enter (void) |
Enter GUIDED mode control. More... | |
void | guidance_v_guided_run (bool in_flight) |
Run GUIDED mode control. More... | |
void | guidance_v_set_z (float z) |
Set z position setpoint. More... | |
void | guidance_v_set_vz (float vz) |
Set z velocity setpoint. More... | |
void | guidance_v_set_th (float th) |
Set throttle setpoint. More... | |
Variables | |
struct VerticalGuidance | guidance_v |
static bool | desired_zd_updated |
static int | guidance_v_guided_mode |
Vertical guidance for rotorcrafts.
Definition in file guidance_v.c.
#define GUIDANCE_V_CLIMB_RC_DEADBAND MAX_PPRZ/10 |
Definition at line 48 of file guidance_v.c.
#define GUIDANCE_V_GUIDED_MODE_CLIMB 1 |
Definition at line 64 of file guidance_v.c.
#define GUIDANCE_V_GUIDED_MODE_THROTTLE 2 |
Definition at line 65 of file guidance_v.c.
#define GUIDANCE_V_GUIDED_MODE_ZHOLD 0 |
Definition at line 63 of file guidance_v.c.
#define GUIDANCE_V_MAX_RC_CLIMB_SPEED GUIDANCE_V_REF_MIN_ZD |
Definition at line 52 of file guidance_v.c.
#define GUIDANCE_V_MAX_RC_DESCENT_SPEED GUIDANCE_V_REF_MAX_ZD |
Definition at line 56 of file guidance_v.c.
#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4 |
Definition at line 42 of file guidance_v.c.
|
static |
get the cosine of the angle between thrust vector and gravity vector
Definition at line 174 of file guidance_v.c.
References BFP_OF_REAL, INT32_TRIG_FRAC, Int32RMat::m, and stateGetNedToBodyRMat_i().
Referenced by guidance_v_thrust_adapt().
void guidance_v_from_nav | ( | bool | in_flight | ) |
Set guidance setpoint from NAV and run hover loop.
Definition at line 316 of file guidance_v.c.
References RotorcraftNavigation::climb, VerticalGuidance::delta_t, guidance_v, guidance_v_guided_run(), guidance_v_run_enter(), guidance_v_run_pos(), guidance_v_run_speed(), guidance_v_update_ref(), GuidanceVSetRef, gv_update_ref_from_z_sp(), gv_update_ref_from_zd_sp(), Min, nav, RotorcraftNavigation::nav_altitude, NAV_VERTICAL_MODE_ALT, NAV_VERTICAL_MODE_CLIMB, NAV_VERTICAL_MODE_GUIDED, NAV_VERTICAL_MODE_MANUAL, POS_BFP_OF_REAL, radio_control, VerticalGuidance::rc_delta_t, RC_OK, SPEED_BFP_OF_REAL, stabilization_cmd, stateGetPositionNed_i(), stateGetSpeedNed_i(), RadioControl::status, RotorcraftNavigation::throttle, RotorcraftNavigation::vertical_mode, NedCoor_i::z, VerticalGuidance::z_sp, and VerticalGuidance::zd_sp.
Referenced by guidance_v_run().
void guidance_v_guided_enter | ( | void | ) |
Enter GUIDED mode control.
Definition at line 348 of file guidance_v.c.
References guidance_v_run_enter(), guidance_v_set_z(), GuidanceVSetRef, stateGetPositionNed_f(), stateGetPositionNed_i(), and stateGetSpeedNed_i().
Referenced by guidance_v_mode_changed().
void guidance_v_guided_run | ( | bool | in_flight | ) |
Run GUIDED mode control.
Definition at line 358 of file guidance_v.c.
References VerticalGuidance::delta_t, guidance_v, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_CLIMB, GUIDANCE_V_GUIDED_MODE_THROTTLE, GUIDANCE_V_GUIDED_MODE_ZHOLD, guidance_v_run_pos(), guidance_v_run_speed(), guidance_v_update_ref(), gv_update_ref_from_z_sp(), gv_update_ref_from_zd_sp(), Min, radio_control, VerticalGuidance::rc_delta_t, RC_OK, stabilization_cmd, stateGetPositionNed_i(), RadioControl::status, VerticalGuidance::th_sp, NedCoor_i::z, VerticalGuidance::z_sp, and VerticalGuidance::zd_sp.
Referenced by guidance_v_from_nav(), and guidance_v_run().
void guidance_v_init | ( | void | ) |
Definition at line 83 of file guidance_v.c.
References BFP_OF_REAL, DefaultPeriodic, desired_zd_updated, guidance_v, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_ZHOLD, GUIDANCE_V_MODE_KILL, guidance_v_module_init(), gv_adapt_init(), INT32_TRIG_FRAC, VerticalGuidance::mode, register_periodic_telemetry(), send_tune_vert(), and VerticalGuidance::thrust_coeff.
void guidance_v_mode_changed | ( | uint8_t | new_mode | ) |
Definition at line 126 of file guidance_v.c.
References guidance_v, guidance_v_guided_enter(), GUIDANCE_V_MODE_CLIMB, GUIDANCE_V_MODE_FLIP, GUIDANCE_V_MODE_GUIDED, GUIDANCE_V_MODE_HOVER, GUIDANCE_V_MODE_MODULE, GUIDANCE_V_MODE_NAV, GUIDANCE_V_MODE_RC_CLIMB, guidance_v_module_enter(), guidance_v_run_enter(), GuidanceVSetRef, VerticalGuidance::mode, stateGetPositionNed_i(), stateGetSpeedNed_i(), and VerticalGuidance::zd_sp.
Referenced by autopilot_static_set_mode().
void guidance_v_notify_in_flight | ( | bool | in_flight | ) |
Definition at line 166 of file guidance_v.c.
References gv_adapt_init().
void guidance_v_read_rc | ( | void | ) |
Definition at line 104 of file guidance_v.c.
References guidance_v, GUIDANCE_V_CLIMB_RC_DEADBAND, GUIDANCE_V_MAX_RC_CLIMB_SPEED, GUIDANCE_V_MAX_RC_DESCENT_SPEED, MAX_PPRZ, radio_control, RADIO_THROTTLE, VerticalGuidance::rc_delta_t, VerticalGuidance::rc_zd_sp, SPEED_BFP_OF_REAL, and RadioControl::values.
Referenced by autopilot_static_on_rc_frame().
void guidance_v_run | ( | bool | in_flight | ) |
Definition at line 223 of file guidance_v.c.
References VerticalGuidance::delta_t, desired_zd_updated, guidance_v, guidance_v_from_nav(), guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_ZHOLD, guidance_v_guided_run(), GUIDANCE_V_MODE_CLIMB, GUIDANCE_V_MODE_FLIP, GUIDANCE_V_MODE_GUIDED, GUIDANCE_V_MODE_HOVER, GUIDANCE_V_MODE_MODULE, GUIDANCE_V_MODE_NAV, GUIDANCE_V_MODE_RC_CLIMB, GUIDANCE_V_MODE_RC_DIRECT, guidance_v_module_run(), guidance_v_run_speed(), guidance_v_thrust_adapt(), gv_update_ref_from_zd_sp(), Min, VerticalGuidance::mode, radio_control, VerticalGuidance::rc_delta_t, RC_OK, VerticalGuidance::rc_zd_sp, stabilization_cmd, stateGetPositionNed_i(), RadioControl::status, NedCoor_i::z, VerticalGuidance::z_sp, and VerticalGuidance::zd_sp.
Referenced by autopilot_static_periodic().
Set guidance ref parameters.
Definition at line 296 of file guidance_v.c.
References guidance_v, gv_set_ref(), VerticalGuidance::z_ref, VerticalGuidance::zd_ref, and VerticalGuidance::zdd_ref.
void guidance_v_set_th | ( | float | th | ) |
Set throttle setpoint.
th | Throttle setpoint between 0. and 1. |
Definition at line 410 of file guidance_v.c.
References guidance_v, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_THROTTLE, GuidanceVSetRef, MAX_PPRZ, stateGetPositionNed_i(), stateGetSpeedNed_i(), and VerticalGuidance::th_sp.
void guidance_v_set_vz | ( | float | vz | ) |
Set z velocity setpoint.
vz | Setpoint (down is positive) in meters/second. |
Definition at line 402 of file guidance_v.c.
References guidance_v, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_CLIMB, SPEED_BFP_OF_REAL, and VerticalGuidance::zd_sp.
Referenced by autopilot_guided_move_ned(), autopilot_guided_update(), and autopilot_static_set_mode().
void guidance_v_set_z | ( | float | z | ) |
Set z position setpoint.
z | Setpoint (down is positive) in meters. |
Definition at line 392 of file guidance_v.c.
References guidance_v, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_ZHOLD, POS_BFP_OF_REAL, VerticalGuidance::z_sp, and VerticalGuidance::zd_sp.
Referenced by autopilot_guided_goto_ned(), autopilot_guided_update(), and guidance_v_guided_enter().
void guidance_v_thrust_adapt | ( | bool | in_flight | ) |
Definition at line 202 of file guidance_v.c.
References desired_zd_updated, get_vertical_thrust_coeff(), guidance_v, gv_adapt_init(), gv_adapt_run(), INT32_TRIG_FRAC, stabilization_cmd, stateGetAccelNed_i(), VerticalGuidance::thrust_coeff, and VerticalGuidance::zd_ref.
Referenced by guidance_v_run().
void guidance_v_update_ref | ( | void | ) |
Definition at line 305 of file guidance_v.c.
References desired_zd_updated, guidance_v, gv_z_ref, GV_Z_REF_FRAC, gv_zd_ref, GV_ZD_REF_FRAC, gv_zdd_ref, GV_ZDD_REF_FRAC, INT32_ACCEL_FRAC, INT32_POS_FRAC, INT32_SPEED_FRAC, VerticalGuidance::z_ref, VerticalGuidance::zd_ref, and VerticalGuidance::zdd_ref.
Referenced by guidance_v_from_nav(), and guidance_v_guided_run().
void guidance_v_z_enter | ( | void | ) |
Definition at line 283 of file guidance_v.c.
References guidance_v, guidance_v_run_enter(), GuidanceVSetRef, stateGetPositionNed_i(), NedCoor_i::z, VerticalGuidance::z_sp, and VerticalGuidance::zd_sp.
|
static |
Definition at line 73 of file guidance_v.c.
References dev, guidance_v, stateGetPositionNed_i(), VerticalGuidance::z_ref, VerticalGuidance::z_sp, and VerticalGuidance::zd_ref.
Referenced by guidance_v_init().
|
static |
Definition at line 61 of file guidance_v.c.
Referenced by guidance_v_init(), guidance_v_run(), guidance_v_thrust_adapt(), and guidance_v_update_ref().
struct VerticalGuidance guidance_v |
Definition at line 1 of file guidance_v.c.
Referenced by guidance_hybrid_vertical(), guidance_v_from_nav(), guidance_v_guided_run(), guidance_v_init(), guidance_v_mode_changed(), guidance_v_read_rc(), guidance_v_run(), guidance_v_set_ref(), guidance_v_set_th(), guidance_v_set_vz(), guidance_v_set_z(), guidance_v_thrust_adapt(), guidance_v_update_ref(), guidance_v_z_enter(), send_fp(), send_status(), send_tune_vert(), send_vert_loop(), and vertical_ctrl_module_init().
|
static |
Definition at line 67 of file guidance_v.c.
Referenced by guidance_v_guided_run(), guidance_v_init(), guidance_v_run(), guidance_v_set_th(), guidance_v_set_vz(), and guidance_v_set_z().