 |
Paparazzi UAS
v6.1.0_stable
Paparazzi is a free software Unmanned Aircraft System.
|
Go to the documentation of this file.
30 #include "generated/airframe.h"
54 #ifndef GUIDANCE_INDI_SPEED_GAIN
55 #define GUIDANCE_INDI_SPEED_GAIN 1.8
56 #define GUIDANCE_INDI_SPEED_GAINZ 1.8
59 #ifndef GUIDANCE_INDI_POS_GAIN
60 #define GUIDANCE_INDI_POS_GAIN 0.5
61 #define GUIDANCE_INDI_POS_GAINZ 0.5
71 .heading_bank_gain = GUIDANCE_INDI_HEADING_BANK_GAIN,
74 #ifndef GUIDANCE_INDI_MAX_AIRSPEED
75 #error "You must have an airspeed sensor to use this guidance"
80 #define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + 10.0)
83 #ifndef MAX_DECELERATION
84 #define MAX_DECELERATION 1.
91 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
93 static void guidance_indi_filter_thrust(
void);
95 #ifndef GUIDANCE_INDI_THRUST_DYNAMICS
96 #ifndef STABILIZATION_INDI_ACT_DYN_P
97 #error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS to be able to use indi vertical control"
98 #else // assume that the same actuators are used for thrust as for roll (e.g. quadrotor)
99 #define GUIDANCE_INDI_THRUST_DYNAMICS STABILIZATION_INDI_ACT_DYN_P
101 #endif //GUIDANCE_INDI_THRUST_DYNAMICS
103 #endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
105 #ifndef GUIDANCE_INDI_FILTER_CUTOFF
106 #ifdef STABILIZATION_INDI_FILT_CUTOFF
107 #define GUIDANCE_INDI_FILTER_CUTOFF STABILIZATION_INDI_FILT_CUTOFF
109 #define GUIDANCE_INDI_FILTER_CUTOFF 3.0
113 #ifdef GUIDANCE_INDI_LINE_GAIN
156 #if PERIODIC_TELEMETRY
160 pprz_msg_send_GUIDANCE_INDI_HYBRID(trans,
dev, AC_ID,
184 float sample_time = 1.0/PERIODIC_FREQUENCY;
185 for(
int8_t i=0; i<3; i++) {
193 #if PERIODIC_TELEMETRY
207 float sample_time = 1.0 / PERIODIC_FREQUENCY;
208 for (
int8_t i = 0; i < 3; i++) {
260 struct FloatVect2 airspeed_v = {cos(psi)*airspeed, sin(psi)*airspeed};
262 VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
268 if((airspeed > 10.0) && (norm_des_as > 12.0)) {
272 float groundspeed_factor = 0.0;
280 float dv = bv * bv - 4.0 * av * cv;
286 float d_sqrt = sqrtf(dv);
288 groundspeed_factor = (-bv + d_sqrt) / (2.0 * av);
314 sp_accel.
x = cosf(psi) * sp_accel_b.
x - sinf(psi) * sp_accel_b.
y;
315 sp_accel.
y = sinf(psi) * sp_accel_b.
x + cosf(psi) * sp_accel_b.
y;
320 if(airspeed > 10.0) {
323 float speed_increment = speed_sp_b_x - groundspeed_x;
330 speed_sp.
x = cosf(psi) * speed_sp_b_x - sinf(psi) * speed_sp_b_y;
331 speed_sp.
y = sinf(psi) * speed_sp_b_x + cosf(psi) * speed_sp_b_y;
345 #if GUIDANCE_INDI_RC_DEBUG
346 #warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
351 sp_accel.
x = cosf(psi) * rc_x - sinf(psi) * rc_y;
352 sp_accel.
y = sinf(psi) * rc_x + cosf(psi) * rc_y;
374 Bound(a_diff.
x, -6.0, 6.0);
375 Bound(a_diff.
y, -6.0, 6.0);
376 Bound(a_diff.
z, -9.0, 9.0);
380 #ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
381 #ifndef STABILIZATION_ATTITUDE_INDI_FULL
394 const float max_phi = RadOfDeg(60.0);
395 float airspeed_turn = airspeed;
397 Bound(airspeed_turn,10.0,30.0);
413 if (fabs(coordinated_turn_roll) < max_phi) {
414 omega = 9.81 / airspeed_turn * tanf(coordinated_turn_roll);
416 omega = 9.81 / airspeed_turn * 1.72305 * ((coordinated_turn_roll > 0.0) - (coordinated_turn_roll < 0.0));
419 #ifdef FWD_SIDESLIP_GAIN
426 #ifndef KNIFE_EDGE_TEST
430 *heading_sp += omega / PERIODIC_FREQUENCY;
437 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
438 guidance_indi_filter_thrust();
442 Bound(
thrust_in, GUIDANCE_INDI_MIN_THROTTLE, 9600);
444 #if GUIDANCE_INDI_RC_DEBUG
461 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
465 void guidance_indi_filter_thrust(
void)
513 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
514 #define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
519 Bound(pitch_lift,-M_PI_2,0);
520 float lift = sinf(pitch_lift)*9.81;
521 float T = cosf(pitch_lift)*-9.81;
526 RMAT_ELMT(*Gmat, 0, 0) = cphi*ctheta*spsi*
T + cphi*spsi*lift;
527 RMAT_ELMT(*Gmat, 1, 0) = -cphi*ctheta*cpsi*
T - cphi*cpsi*lift;
528 RMAT_ELMT(*Gmat, 2, 0) = -sphi*ctheta*
T -sphi*lift;
532 RMAT_ELMT(*Gmat, 0, 2) = stheta*cpsi + sphi*ctheta*spsi;
533 RMAT_ELMT(*Gmat, 1, 2) = stheta*spsi - sphi*ctheta*cpsi;
547 float pitch_interp = DegOfRad(theta);
548 Bound(pitch_interp, -80.0, -40.0);
549 float ratio = (pitch_interp + 40.0)/(-40.);
589 struct FloatVect2 line_v = {line_v_enu.
y, line_v_enu.x};
590 struct FloatVect2 to_end_v = {to_end_v_enu.
y, to_end_v_enu.x};
602 desired_speed = dist_to_target * pos_gain;
608 if(length_line < 0.01) {
616 float length_normalv = length_line;
617 if(length_normalv < 0.01) {
618 length_normalv = 0.01;
622 float dist_to_line = (to_end_v.
x*normalv.
x + to_end_v.
y*normalv.
y)/length_normalv;
630 float dist_to_line_abs = fabs(dist_to_line);
634 v_along_line.
x = line_v.
x/length_line*50.0;
635 v_along_line.
y = line_v.
y/length_line*50.0;
642 if(length_direction < 0.01) {
643 length_direction = 0.01;
657 Bound(speed_sp_return.
z, -4.0, 5.0);
662 return speed_sp_return;
684 VECT3_SMUL(speed_sp_return, pos_error, pos_gain);
701 if(max_speed_decel2 < 0.0) {
702 max_speed_decel2 = fabs(max_speed_decel2);
704 float max_speed_decel = sqrtf(max_speed_decel2);
713 Bound(speed_sp_return.
z, -4.0, 5.0);
718 return speed_sp_return;
struct FloatVect3 speed_sp
#define GUIDANCE_INDI_SPEED_GAINZ
struct FloatVect2 desired_airspeed
static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat)
Calculate the matrix of partial derivatives of the roll, pitch and thrust w.r.t.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
signed char int8_t
Typedef defining 8 bit char type.
vector in North East Down coordinates Units: meters
struct HorizontalGuidanceReference ref
reference calculated from setpoints
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
#define VECT2_SMUL(_vo, _vi, _s)
#define ANGLE_FLOAT_OF_BFP(_ai)
int32_t transition_theta_offset
#define FLOAT_ANGLE_NORMALIZE(_a)
Butterworth2LowPass accely_filt
static float guidance_indi_get_liftd(float pitch, float theta)
Get the derivative of lift w.r.t.
#define THRUST_INCREMENT_ID
#define VECT3_SMUL(_vo, _vi, _s)
#define GUIDANCE_H_MAX_BANK
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
uint8_t mode
current autopilot mode
bool take_heading_control
#define GUIDANCE_INDI_PITCH_EFF_SCALING
Butterworth2LowPass filt_accel_ned[3]
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Butterworth2LowPass thrust_filt
struct EnuCoor_i navigation_target
#define VECT3_DIFF(_c, _a, _b)
Second order low pass filter structure.
struct guidance_indi_hybrid_params gih_params
#define POS_FLOAT_OF_BFP(_ai)
#define GUIDANCE_INDI_POS_GAINZ
Butterworth2LowPass roll_filt
#define SPEED_FLOAT_OF_BFP(_ai)
static const struct usb_device_descriptor dev
#define VECT2_ADD(_a, _b)
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
int32_t nav_heading
with INT32_ANGLE_FRAC
#define VERTICAL_MODE_CLIMB
#define VECT2_DIFF(_c, _a, _b)
static float float_vect2_norm(struct FloatVect2 *v)
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e)
quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
#define ACCEL_FLOAT_OF_BFP(_ai)
struct pprz_autopilot autopilot
Global autopilot structure.
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
#define FLOAT_VECT2_NORM(_v)
#define ANGLE_BFP_OF_REAL(_af)
static float stateGetAirspeed_f(void)
Get airspeed (float).
void vect_scale(struct FloatVect3 *vect3, float norm_des)
int32_t guidance_v_zd_sp
vertical speed setpoint in meter/s (input).
#define INT_MULT_RSHIFT(_a, _b, _r)
static void float_quat_normalize(struct FloatQuat *q)
struct FloatVect2 line_vect to_end_vect
Architecture independent timing functions.
#define RMAT_ELMT(_rm, _row, _col)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
#define QUAT_BFP_OF_REAL(_qi, _qf)
float guidance_indi_max_bank
#define BFP_OF_REAL(_vr, _frac)
#define GUIDANCE_INDI_FILTER_CUTOFF
void guidance_indi_propagate_filters(void)
Low pass the accelerometer measurements to remove noise from vibrations.
#define GUIDANCE_INDI_SPEED_GAIN
#define MAT33_INV(_minv, _m)
#define VECT2_ASSIGN(_a, _x, _y)
struct FloatVect3 nav_get_speed_sp_from_line(struct FloatVect2 line_v_enu, struct FloatVect2 to_end_v_enu, struct EnuCoor_i target, float pos_gain)
follow a line.
struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain)
Go to a waypoint in the shortest way.
int int32_t
Typedef defining 32 bit int type.
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
static void send_guidance_indi_hybrid(struct transport_tx *trans, struct link_device *dev)
float guidance_indi_specific_force_gain
void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
struct FloatVect3 euler_cmd
void vect_bound_in_2d(struct FloatVect3 *vect3, float bound)
float guidance_indi_line_gain
#define GUIDANCE_V_MODE_NAV
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
void guidance_indi_enter(void)
Call upon entering indi guidance.
int32_t transition_percentage
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
struct Int32Vect2 pos
with INT32_POS_FRAC
struct HorizontalGuidance guidance_h
float guidance_indi_max_airspeed
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
struct FloatVect3 nav_get_speed_setpoint(float pos_gain)
function that returns a speed setpoint based on flight plan.
struct FloatEulers eulers_zxy
state eulers in zxy order
void guidance_indi_run(float *heading_sp)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define INT32_PERCENTAGE_FRAC
#define HORIZONTAL_MODE_ROUTE
void guidance_indi_init(void)
Init function.
#define DefaultPeriodic
Set default periodic telemetry.
vector in East North Up coordinates
Butterworth2LowPass pitch_filt
struct RadioControl radio_control
struct FloatVect3 sp_accel
#define GUIDANCE_INDI_POS_GAIN
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
struct FloatEulers guidance_euler_cmd
Simple first order low pass filter with bilinear transform.
int32_t guidance_v_z_ref
altitude reference in meters.