28 #include "generated/modules.h"
35 #define DEBUG_AMT TRUE
61 #if PERIODIC_TELEMETRY
65 pprz_msg_send_APPROACH_MOVING_TARGET(trans,
dev, AC_ID,
78 #if PERIODIC_TELEMETRY
119 struct NedCoor_f target_pos, target_vel = {0};
120 float target_heading;
130 float psi_ref = RadOfDeg(target_heading +
amt.
psi_ref);
142 VECT3_SUM(des_pos, ref_relpos, target_pos);
158 ref_relvel.
x + target_vel.
x + ec_vel.
x,
159 ref_relvel.
y + target_vel.
y + ec_vel.
y,
160 ref_relvel.
z + target_vel.
z + ec_vel.
z,
167 Bound(des_vel.
z, -4.0, 5.0);
178 float sin_gamma_ref = sinf(gamma_ref);
179 if (sin_gamma_ref > 0.05) {
193 float dt = FOLLOW_DIAGONAL_APPROACH_PERIOD;
198 VECT3_SUM(disp_pos_target, des_pos, *drone_pos);
Main include for ABI (AirBorneInterface).
float approach_moving_target_angle_deg
void follow_diagonal_approach(void)
Generates a velocity reference from a diagonal approach path.
struct FloatVect3 nav_get_speed_sp_from_diagonal(struct EnuCoor_i target, float pos_gain, float rope_heading)
struct AmtTelem amt_telem
float amt_err_slowdown_gain
struct FloatVect3 des_vel
bool approach_moving_target_enabled
struct FloatVect3 des_pos
static void send_approach_moving_target(struct transport_tx *trans, struct link_device *dev)
void approach_moving_target_init(void)
void update_waypoint(uint8_t wp_id, struct FloatVect3 *target_ned)
void approach_moving_target_enable(uint8_t wp_id)
struct FloatVect3 rel_unit_vec
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
void vect_bound_in_3d(struct FloatVect3 *vect3, float bound)
#define VECT3_SUM(_c, _a, _b)
#define VECT3_SMUL(_vo, _vi, _s)
#define VECT3_COPY(_a, _b)
#define VECT3_DIFF(_c, _a, _b)
#define ENU_OF_TO_NED(_po, _pi)
vector in East North Up coordinates
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
static float stateGetAirspeed_f(void)
Get airspeed (float).
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
vector in East North Up coordinates Units: meters
vector in North East Down coordinates Units: meters
static const struct usb_device_descriptor dev
bool target_get_pos(struct NedCoor_f *pos, float *heading)
Get the current target position (NED) and heading.
bool target_get_vel(struct NedCoor_f *vel)
Get the current target velocity (NED)
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.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.