44 #define MAX_AIRSPEED 15
46 #define INT32_ANGLE_HIGH_RES_FRAC 18
75 #if PERIODIC_TELEMETRY
82 pprz_msg_send_HYBRID_GUIDANCE(trans, dev, AC_ID,
84 &(speed->
x), &(speed->
y),
118 #if PERIODIC_TELEMETRY
124 #define INT32_ANGLE_HIGH_RES_NORMALIZE(_a) { \
125 while ((_a) > (INT32_ANGLE_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC))) (_a) -= (INT32_ANGLE_2_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)); \
126 while ((_a) < (-INT32_ANGLE_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC))) (_a) += (INT32_ANGLE_2_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)); \
194 if (heading_diff > 0.0) {
196 }
else if (heading_diff < 0.0) {
206 if (norm_sp_airspeed > (4 << 8)) {
234 if (norm_sp_airspeed < (4 << 8)) {
270 struct Int32Vect2 guidance_hybrid_groundspeed_sp;
279 if (norm_groundspeed_sp > 1 << 8) {
280 guidance_hybrid_groundspeed_sp.
x = guidance_hybrid_groundspeed_sp.
x * (
max_airspeed << 8) / norm_groundspeed_sp;
281 guidance_hybrid_groundspeed_sp.
y = guidance_hybrid_groundspeed_sp.
y * (
max_airspeed << 8) / norm_groundspeed_sp;
294 VECT2_ADD(airspeed_sp, guidance_hybrid_groundspeed_sp);
301 if (norm_airspeed_sp > (
max_airspeed << 8) && norm_groundspeed_sp > 0) {
303 8) +
INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.
y, guidance_hybrid_groundspeed_sp.
y, 8);
310 float d_sqrt_f = sqrtf(dv);
313 int32_t result = ((-bv + d_sqrt) << 8) / (2 * av);
326 if ((norm_airspeed_sp > (
max_airspeed << 8)) && (norm_airspeed_sp != 0)) {
int32_t psi
in rad with INT32_ANGLE_FRAC
#define FLOAT_ANGLE_NORMALIZE(_a)
static int32_t v_control_pitch
void guidance_hybrid_vertical(void)
Description.
int32_t guidance_v_kd
vertical control D-gain
static struct Int32Vect2 wind_estimate_high_res
void guidance_hybrid_position_to_airspeed(void)
Description.
void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp)
Convert a required airspeed to a certain attitude for the Quadshot.
struct Int32Vect2 pos
horizontal position setpoint in NED.
void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
#define INT32_ANGLE_HIGH_RES_NORMALIZE(_a)
void int32_quat_of_axis_angle(struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle)
Quaternion from unit vector and angle.
Periodic telemetry system header (includes downlink utility and generated code).
General attitude stabilization interface for rotorcrafts.
#define INT32_ANGLE_HIGH_RES_FRAC
int32_t guidance_v_delta_t
thrust command.
int32_t theta
in rad with INT32_ANGLE_FRAC
void guidance_hybrid_init(void)
Hybrid Guidance Initialization function.
int32_t guidance_v_kp
vertical control P-gain
#define POS_BFP_OF_REAL(_af)
static struct Int32Eulers guidance_hybrid_ypr_sp
static int32_t airspeed_sp_heading_disp
float guidance_v_nominal_throttle
nominal throttle for hover.
Vertical guidance for rotorcrafts.
#define VECT2_DIFF(_c, _a, _b)
static struct Int32Vect2 guidance_hybrid_ref_airspeed
static struct Int32Vect2 guidance_hybrid_airspeed_ref
static uint32_t int32_vect2_norm(struct Int32Vect2 *v)
return norm of 2D vector
Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
void guidance_hybrid_run(void)
Runs the Hybrid Guidance main functions.
static void send_hybrid_guidance(struct transport_tx *trans, struct link_device *dev)
#define VECT2_ADD(_a, _b)
#define ANGLE_FLOAT_OF_BFP(_ai)
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
void guidance_hybrid_determine_wind_estimate(void)
Description.
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
#define PPRZ_ITRIG_SIN(_s, _a)
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
int32_t horizontal_speed_gain
int32_t guidance_v_ki
vertical control I-gain
static int32_t heading_diff_disp
static int32_t omega_disp
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
#define QUAT_BFP_OF_REAL(_qi, _qf)
void guidance_hybrid_reset_heading(struct Int32Eulers *sp_cmd)
Description.
#define DefaultPeriodic
Set default periodic telemetry.
struct HorizontalGuidance guidance_h
static int32_t norm_sp_airspeed_disp
int32_t guidance_hybrid_norm_ref_airspeed
void guidance_hybrid_set_cmd_i(struct Int32Eulers *sp_cmd)
Creates the attitude set-points from an orientation vector.
#define ANGLE_BFP_OF_REAL(_af)
static const struct usb_device_descriptor dev
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
#define VECT2_SDIV(_vo, _vi, _s)
static int32_t high_res_psi
int32_t phi
in rad with INT32_ANGLE_FRAC
#define INT_EULERS_ZERO(_e)
vector in North East Down coordinates
#define INT_MULT_RSHIFT(_a, _b, _r)
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
static struct Int32Vect2 guidance_hybrid_airspeed_sp
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
#define INT_VECT2_ZERO(_v)
#define POS_FLOAT_OF_BFP(_ai)
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
static struct Int32Vect2 guidance_h_pos_err
Horizontal guidance for rotorcrafts.
#define INT32_VECT2_RSHIFT(_o, _i, _r)
static bool force_forward_flight
struct HorizontalGuidanceSetpoint sp
setpoints
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
static bool guidance_hovering
#define PPRZ_ITRIG_COS(_c, _a)
static struct Int32Vect2 wind_estimate