27 #include "generated/airframe.h"
63 #if PERIODIC_TELEMETRY
66 static void send_gh(
struct transport_tx *trans,
struct link_device *
dev)
69 pprz_msg_send_GUIDANCE_H_INT(trans,
dev, AC_ID,
72 &(pos->
x), &(pos->
y));
75 static void send_href(
struct transport_tx *trans,
struct link_device *
dev)
77 pprz_msg_send_GUIDANCE_H_REF_INT(trans,
dev, AC_ID,
86 #if defined(COMMAND_ROLL) && defined(COMMAND_PITCH) && defined(COMMAND_YAW)
89 pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans,
dev, AC_ID,
124 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
128 #if PERIODIC_TELEMETRY
162 #if USE_STABILIZATION_RATE
173 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
185 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
194 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
202 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
233 #if USE_STABILIZATION_RATE
235 #if SWITCH_STICKS_FOR_RATE_CONTROL
254 #if GUIDANCE_H_USE_SPEED_REF
261 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
291 #if USE_STABILIZATION_RATE
308 #if (STABILIZATION_FILTER_CMD_ROLL_PITCH || STABILIZATION_FILTER_CMD_YAW)
330 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
349 #if GUIDANCE_H_USE_REF
403 #if GUIDANCE_H_USE_SPEED_REF
525 #ifdef TRANSITION_MAX_OFFSET
598 float newvx = cosf(-
psi) * vx + sinf(-
psi) * vy;
599 float newvy = -sinf(-
psi) * vx + cosf(-
psi) * vy;
616 float newax = cosf(-
psi) * ax + sinf(-
psi) * ay;
617 float neway = -sinf(-
psi) * ax + cosf(-
psi) * ay;
void guidance_h_module_read_rc(void)
void guidance_h_module_run(bool in_flight)
void guidance_h_module_init(void)
void guidance_h_module_enter(void)
#define FLOAT_ANGLE_NORMALIZE(_a)
#define FLOAT_EULERS_ZERO(_e)
#define FLOAT_VECT2_ZERO(_v)
#define VECT2_COPY(_a, _b)
#define QUAT_BFP_OF_REAL(_qi, _qf)
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
#define INT_MULT_RSHIFT(_a, _b, _r)
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_PERCENTAGE_FRAC
#define ACCEL_BFP_OF_REAL(_af)
#define SPEED_FLOAT_OF_BFP(_ai)
#define INT_VECT2_ZERO(_v)
#define INT32_VECT2_RSHIFT(_o, _i, _r)
#define POS_BFP_OF_REAL(_af)
#define ACCEL_FLOAT_OF_BFP(_ai)
#define SPEED_BFP_OF_REAL(_af)
vector in North East Down coordinates
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
void guidance_flip_run(void)
void guidance_flip_enter(void)
Open Loop guidance for making a flip.
void gh_set_ref(struct Int32Vect2 pos, struct FloatVect2 speed, struct FloatVect2 accel)
void gh_update_ref_from_accel_sp(struct FloatVect2 accel_sp)
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp)
void gh_update_ref_from_speed_sp(struct FloatVect2 speed_sp)
struct GuidanceHRef gh_ref
#define GH_POS_REF_FRAC
fixedpoint representation: Q26.37 will give a range of 67e3 km and a resolution of 1....
struct FloatVect2 accel
Reference model acceleration.
#define GUIDANCE_H_REF_MAX_SPEED
Default speed saturation.
struct Int64Vect2 pos
Reference model position.
struct FloatVect2 speed
Reference model speed.
Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
struct FloatVect3 speed_sp
Guidance in a module file.
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
struct RadioControl radio_control
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
#define RADIO_ROLL
Redefining RADIO_* Do not use with radio.h (ppm rc)
int32_t transition_percentage
void guidance_h_set_acc(float ax, float ay)
Set horizontal velocity setpoint.
struct StabilizationSetpoint guidance_h_cmd
horizontal guidance command.
void guidance_h_read_rc(bool in_flight)
void guidance_h_set_pos(float x, float y)
Set horizontal position setpoint.
void guidance_h_mode_changed(uint8_t new_mode)
static void send_href(struct transport_tx *trans, struct link_device *dev)
static void send_gh(struct transport_tx *trans, struct link_device *dev)
void guidance_h_hover_enter(void)
static void send_tune_hover(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED)
void guidance_h_set_body_acc(float ax, float ay)
Set body relative horizontal acceleration setpoint.
void guidance_h_guided_run(bool in_flight)
Run GUIDED mode control.
void guidance_h_from_nav(bool in_flight)
Set horizontal guidance from NAV and run control loop.
void guidance_h_set_heading(float heading)
Set heading setpoint.
void guidance_h_init(void)
struct HorizontalGuidance guidance_h
static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight)
read speed setpoint from RC
void guidance_h_nav_enter(void)
static void guidance_h_update_reference(void)
void guidance_h_set_vel(float vx, float vy)
Set horizontal acceleration setpoint.
void guidance_h_set_body_vel(float vx, float vy)
Set body relative horizontal velocity setpoint.
static void reset_guidance_reference_from_current_position(void)
static void transition_run(bool to_forward)
void guidance_h_set_heading_rate(float rate)
Set heading rate setpoint.
void guidance_h_run(bool in_flight)
Horizontal guidance for rotorcrafts.
#define GUIDANCE_H_MODE_FORWARD
#define GUIDANCE_H_MODE_NAV
#define GUIDANCE_H_USE_REF
Use horizontal guidance reference trajectory.
struct Int32Vect2 accel
with INT32_ACCEL_FRAC
void guidance_h_run_enter(void)
#define GUIDANCE_H_MODE_HOVER
struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
struct Int32Vect2 speed
with INT32_SPEED_FRAC
struct FloatEulers rc_sp
remote control setpoint
#define GUIDANCE_H_MODE_ATTITUDE
struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
#define GUIDANCE_H_MODE_GUIDED
#define GUIDANCE_H_MODE_RC_DIRECT
struct Int32Vect2 pos
with INT32_POS_FRAC
struct HorizontalGuidanceReference ref
reference calculated from setpoints
#define GUIDANCE_H_USE_SPEED_REF
Use horizontal guidance speed reference.
#define GUIDANCE_H_MODE_CARE_FREE
struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh)
#define GUIDANCE_H_MODE_KILL
#define GUIDANCE_H_MODE_RATE
struct HorizontalGuidanceSetpoint sp
setpoints
#define GUIDANCE_H_MODE_FLIP
#define GUIDANCE_H_MODE_MODULE
Vertical guidance for rotorcrafts.
struct RotorcraftNavigation nav
Rotorcraft navigation functions.
#define NAV_SETPOINT_MODE_QUAT
struct EnuCoor_f speed
speed setpoint (in m/s)
#define NAV_SETPOINT_MODE_SPEED
struct FloatQuat quat
quaternion setpoint
struct EnuCoor_f accel
accel setpoint (in m/s)
#define NAV_SETPOINT_MODE_ACCEL
#define NAV_HORIZONTAL_MODE_GUIDED
#define NAV_HORIZONTAL_MODE_ATTITUDE
float pitch
pitch angle (in radians)
#define NAV_HORIZONTAL_MODE_NONE
float heading
heading setpoint (in radians)
struct EnuCoor_f carrot
carrot position (also used for GCS display)
#define NAV_SETPOINT_MODE_POS
Nav setpoint modes these modes correspond to submodes defined by navigation routines to tell which se...
float roll
roll angle (in radians)
General attitude stabilization interface for rotorcrafts.
void stabilization_attitude_enter(void)
void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
void stabilization_attitude_run(bool in_flight)
void stabilization_filter_commands(void)
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
General stabilization interface for rotorcrafts.
void stabilization_attitude_reset_care_free_heading(void)
reset the heading for care-free mode to current heading
int32_t transition_theta_offset
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read an attitude setpoint from the RC.
void stabilization_none_read_rc(void)
void stabilization_none_enter(void)
void stabilization_none_run(bool in_flight)
Dummy stabilization for rotorcrafts.
void stabilization_rate_read_rc(void)
void stabilization_rate_enter(void)
void stabilization_rate_read_rc_switched_sticks(void)
void stabilization_rate_run(bool in_flight)
Rate stabilization for rotorcrafts.
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
enum HorizontalGuidanceSetpoint::@269 h_mask
enum HorizontalGuidanceSetpoint::@270 yaw_mask
struct Int32Vect2 pos
horizontal position setpoint in NED.
struct Int32Vect2 accel
For direct control of acceleration, if the guidance scheme is able to provide it.
struct Int32Vect2 speed
only used in HOVER mode if GUIDANCE_H_USE_SPEED_REF or in GUIDED mode
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.