Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Data Structures | Macros | Functions | Variables
approach_moving_target.c File Reference
#include "generated/airframe.h"
#include "approach_moving_target.h"
#include "generated/modules.h"
#include "modules/core/abi.h"
#include "filters/low_pass_filter.h"
#include <stdio.h>
#include "modules/datalink/telemetry.h"
+ Include dependency graph for approach_moving_target.c:

Go to the source code of this file.

Data Structures

struct  AmtTelem
 

Macros

#define APPROACH_MOVING_TARGET_CUTOFF_FREQ_FILTERS_HZ   0.5
 
#define APPROACH_MOVING_TARGET_SLOPE   35.0
 
#define APPROACH_MOVING_TARGET_DISTANCE   60.0
 
#define APPROACH_MOVING_TARGET_SPEED   -1.0
 
#define APPROACH_MOVING_TARGET_ERR_SLOWDOWN_GAIN   0.25
 
#define APPROACH_MOVING_TARGET_POS_GAIN   0.3
 
#define APPROACH_MOVING_TARGET_SPEED_GAIN   1.0
 
#define APPROACH_MOVING_TARGET_RELVEL_GAIN   1.0
 
#define DEBUG_AMT   TRUE
 

Functions

struct FloatVect3 nav_get_speed_sp_from_diagonal (struct EnuCoor_i target, float pos_gain, float rope_heading)
 
void update_waypoint (uint8_t wp_id, struct FloatVect3 *target_ned)
 
static void send_approach_moving_target (struct transport_tx *trans, struct link_device *dev)
 
void approach_moving_target_init (void)
 
void approach_moving_target_set_low_pass_freq (float filter_freq)
 
void approach_moving_target_enable (uint8_t wp_id)
 
void follow_diagonal_approach (void)
 Generates a velocity reference from a diagonal approach path. More...
 

Variables

Butterworth2LowPass target_pos_filt [3]
 
Butterworth2LowPass target_vel_filt [3]
 
Butterworth2LowPass target_heading_filt
 
struct Amt amt
 
struct AmtTelem amt_telem
 
bool approach_moving_target_enabled = false
 

Detailed Description

Author
Ewoud Smeur e.j.j.nosp@m..sme.nosp@m.ur@tu.nosp@m.delf.nosp@m.t.nl Approach a moving target (e.g. ship)

Definition in file approach_moving_target.c.

Data Structure Documentation

◆ AmtTelem

struct AmtTelem

Definition at line 93 of file approach_moving_target.c.

+ Collaboration diagram for AmtTelem:
Data Fields
struct FloatVect3 des_pos
struct FloatVect3 des_vel

Macro Definition Documentation

◆ APPROACH_MOVING_TARGET_CUTOFF_FREQ_FILTERS_HZ

#define APPROACH_MOVING_TARGET_CUTOFF_FREQ_FILTERS_HZ   0.5

Definition at line 37 of file approach_moving_target.c.

◆ APPROACH_MOVING_TARGET_DISTANCE

#define APPROACH_MOVING_TARGET_DISTANCE   60.0

Definition at line 47 of file approach_moving_target.c.

◆ APPROACH_MOVING_TARGET_ERR_SLOWDOWN_GAIN

#define APPROACH_MOVING_TARGET_ERR_SLOWDOWN_GAIN   0.25

Definition at line 57 of file approach_moving_target.c.

◆ APPROACH_MOVING_TARGET_POS_GAIN

#define APPROACH_MOVING_TARGET_POS_GAIN   0.3

Definition at line 61 of file approach_moving_target.c.

◆ APPROACH_MOVING_TARGET_RELVEL_GAIN

#define APPROACH_MOVING_TARGET_RELVEL_GAIN   1.0

Definition at line 69 of file approach_moving_target.c.

◆ APPROACH_MOVING_TARGET_SLOPE

#define APPROACH_MOVING_TARGET_SLOPE   35.0

Definition at line 42 of file approach_moving_target.c.

◆ APPROACH_MOVING_TARGET_SPEED

#define APPROACH_MOVING_TARGET_SPEED   -1.0

Definition at line 52 of file approach_moving_target.c.

◆ APPROACH_MOVING_TARGET_SPEED_GAIN

#define APPROACH_MOVING_TARGET_SPEED_GAIN   1.0

Definition at line 65 of file approach_moving_target.c.

◆ DEBUG_AMT

#define DEBUG_AMT   TRUE

Definition at line 76 of file approach_moving_target.c.

Function Documentation

◆ approach_moving_target_enable()

void approach_moving_target_enable ( uint8_t  wp_id)

Definition at line 161 of file approach_moving_target.c.

References amt, Amt::enabled_time, get_sys_time_msec(), and Amt::wp_id.

+ Here is the call graph for this function:

◆ approach_moving_target_init()

void approach_moving_target_init ( void  )

Definition at line 119 of file approach_moving_target.c.

References amt, approach_moving_target_set_low_pass_freq(), Amt::cutoff_freq_filters_hz, DefaultPeriodic, register_periodic_telemetry(), and send_approach_moving_target().

+ Here is the call graph for this function:

◆ approach_moving_target_set_low_pass_freq()

void approach_moving_target_set_low_pass_freq ( float  filter_freq)

Definition at line 127 of file approach_moving_target.c.

References amt, Amt::cutoff_freq_filters_hz, init_butterworth_2_low_pass(), target_heading_filt, target_pos_filt, and target_vel_filt.

Referenced by approach_moving_target_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ follow_diagonal_approach()

void follow_diagonal_approach ( void  )

Generates a velocity reference from a diagonal approach path.

Definition at line 171 of file approach_moving_target.c.

◆ nav_get_speed_sp_from_diagonal()

struct FloatVect3 nav_get_speed_sp_from_diagonal ( struct EnuCoor_i  target,
float  pos_gain,
float  rope_heading 
)

◆ send_approach_moving_target()

static void send_approach_moving_target ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 106 of file approach_moving_target.c.

References amt, amt_telem, AmtTelem::des_pos, AmtTelem::des_vel, dev, Amt::distance, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by approach_moving_target_init().

+ Here is the caller graph for this function:

◆ update_waypoint()

void update_waypoint ( uint8_t  wp_id,
struct FloatVect3 *  target_ned 
)

Definition at line 142 of file approach_moving_target.c.

References DefaultChannel, DefaultDevice, ENU_OF_TO_NED, waypoint_set_enu(), waypoints, point::x, and point::y.

+ Here is the call graph for this function:

Variable Documentation

◆ amt

struct Amt amt
Initial value:
= {
.distance = APPROACH_MOVING_TARGET_DISTANCE,
.speed = APPROACH_MOVING_TARGET_SPEED,
.psi_ref = 180.0,
.slope_ref = APPROACH_MOVING_TARGET_SLOPE,
.err_slowdown_gain = APPROACH_MOVING_TARGET_ERR_SLOWDOWN_GAIN,
.pos_gain = APPROACH_MOVING_TARGET_POS_GAIN,
.speed_gain = APPROACH_MOVING_TARGET_SPEED_GAIN,
.relvel_gain = APPROACH_MOVING_TARGET_RELVEL_GAIN,
.cutoff_freq_filters_hz = APPROACH_MOVING_TARGET_CUTOFF_FREQ_FILTERS_HZ,
.enabled_time = 0,
.wp_id = 0,
}
APPROACH_MOVING_TARGET_CUTOFF_FREQ_FILTERS_HZ
#define APPROACH_MOVING_TARGET_CUTOFF_FREQ_FILTERS_HZ
Definition: approach_moving_target.c:37
APPROACH_MOVING_TARGET_POS_GAIN
#define APPROACH_MOVING_TARGET_POS_GAIN
Definition: approach_moving_target.c:61
APPROACH_MOVING_TARGET_RELVEL_GAIN
#define APPROACH_MOVING_TARGET_RELVEL_GAIN
Definition: approach_moving_target.c:69
APPROACH_MOVING_TARGET_SPEED
#define APPROACH_MOVING_TARGET_SPEED
Definition: approach_moving_target.c:52
APPROACH_MOVING_TARGET_SLOPE
#define APPROACH_MOVING_TARGET_SLOPE
Definition: approach_moving_target.c:42
APPROACH_MOVING_TARGET_DISTANCE
#define APPROACH_MOVING_TARGET_DISTANCE
Definition: approach_moving_target.c:47
APPROACH_MOVING_TARGET_SPEED_GAIN
#define APPROACH_MOVING_TARGET_SPEED_GAIN
Definition: approach_moving_target.c:65
APPROACH_MOVING_TARGET_ERR_SLOWDOWN_GAIN
#define APPROACH_MOVING_TARGET_ERR_SLOWDOWN_GAIN
Definition: approach_moving_target.c:57

Definition at line 74 of file approach_moving_target.c.

Referenced by approach_moving_target_enable(), approach_moving_target_init(), approach_moving_target_set_low_pass_freq(), and send_approach_moving_target().

◆ amt_telem

struct AmtTelem amt_telem

Definition at line 74 of file approach_moving_target.c.

Referenced by send_approach_moving_target().

◆ approach_moving_target_enabled

bool approach_moving_target_enabled = false

Definition at line 99 of file approach_moving_target.c.

◆ target_heading_filt

Butterworth2LowPass target_heading_filt

Definition at line 74 of file approach_moving_target.c.

Referenced by approach_moving_target_set_low_pass_freq().

◆ target_pos_filt

Butterworth2LowPass target_pos_filt[3]

Definition at line 72 of file approach_moving_target.c.

Referenced by approach_moving_target_set_low_pass_freq().

◆ target_vel_filt

Butterworth2LowPass target_vel_filt[3]

Definition at line 73 of file approach_moving_target.c.

Referenced by approach_moving_target_set_low_pass_freq().