#include <math.h>
#include <std.h>
#include <stdio.h>
#include "modules/multi/ctc/ctc.h"
#include "subsystems/datalink/telemetry.h"
#include "subsystems/navigation/common_nav.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "autopilot.h"

Include dependency graph for ctc.c:

Macros
#define	CTC_MAX_AC 4

#define	CTC_GAIN_K1 0.001

#define	CTC_GAIN_K2 0.001

#define	CTC_GAIN_ALPHA 0.01

#define	CTC_TIMEOUT 1500

#define	CTC_OMEGA 0.25

#define	CTC_TIME_BROAD 100

Functions
static void	send_ctc (struct transport_tx trans, struct link_device dev)

static void	send_ctc_control (struct transport_tx trans, struct link_device dev)

void	ctc_init (void)

bool	collective_tracking_vehicle ()

bool	collective_tracking_waypoint (uint8_t wp)

bool	collective_tracking_point (float x, float y)

void	collective_tracking_control ()

Variables
ctc_con	ctc_control

int16_t	tableNei [CTC_MAX_AC][6]

uint32_t	last_info [CTC_MAX_AC]

uint32_t	last_transmision = 0

uint32_t	before = 0

uint32_t	time_init_table

float	moving_target_px = 0

float	moving_target_py = 0

float	moving_target_vx = 0

float	moving_target_vy = 0

bool	ctc_gogo = false

bool	ctc_first_time = true

uint32_t	starting_time

Macro Definition Documentation

◆ CTC_GAIN_ALPHA

#define CTC_GAIN_ALPHA 0.01

Definition at line 62 of file ctc.c.

◆ CTC_GAIN_K1

#define CTC_GAIN_K1 0.001

Default gains for the algorithm

Definition at line 56 of file ctc.c.

◆ CTC_GAIN_K2

#define CTC_GAIN_K2 0.001

Definition at line 59 of file ctc.c.

◆ CTC_MAX_AC

#define CTC_MAX_AC 4

Max expected number of aircraft

Definition at line 36 of file ctc.c.

◆ CTC_OMEGA

#define CTC_OMEGA 0.25

Default angular velocity around target in rad/sec

Definition at line 70 of file ctc.c.

◆ CTC_TIME_BROAD

#define CTC_TIME_BROAD 100

Default time in ms for broadcasting information

Definition at line 74 of file ctc.c.

◆ CTC_TIMEOUT

#define CTC_TIMEOUT 1500

Default timeout in ms for the neighbors' information

Definition at line 66 of file ctc.c.

Function Documentation

◆ collective_tracking_control()

void collective_tracking_control ( )

Definition at line 145 of file ctc.c.

References ac_id, ctc_con::alpha, AP_MODE_AUTO2, autopilot_get_mode(), before, ctc_control, ctc_first_time, ctc_gogo, CTC_MAX_AC, ctc_send_info_to_nei(), DefaultChannel, DefaultDevice, get_sys_time_msec(), h_ctl_roll_max_setpoint, h_ctl_roll_setpoint, ctc_con::k1, ctc_con::k2, last_info, last_transmision, lateral_mode, LATERAL_MODE_ROLL, moving_target_px, moving_target_py, moving_target_vx, moving_target_vy, msg, ctc_con::omega, p, ctc_con::p_centroid_x, ctc_con::p_centroid_y, parse_ctc_CleanTable(), parse_ctc_NeiInfoTable(), parse_ctc_RegTable(), parse_ctc_TargetInfo(), ctc_con::px, ctc_con::py, ctc_con::ref_px, ctc_con::ref_py, starting_time, stateGetNedToBodyEulers_f(), stateGetPositionEnu_f(), stateGetSpeedEnu_f(), tableNei, ctc_con::target_px, ctc_con::target_py, ctc_con::target_vx, ctc_con::target_vy, FloatEulers::theta, ctc_con::time_broad, ctc_con::timeout, timeout, ctc_con::v_centroid_x, ctc_con::v_centroid_y, ctc_con::vx, ctc_con::vy, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by collective_tracking_point(), collective_tracking_vehicle(), and collective_tracking_waypoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ collective_tracking_point()

bool collective_tracking_point	(	float	x,
		float	y
	)

Definition at line 133 of file ctc.c.

References collective_tracking_control(), ctc_control, ctc_con::target_px, ctc_con::target_py, ctc_con::target_vx, and ctc_con::target_vy.

Here is the call graph for this function:

◆ collective_tracking_vehicle()

bool collective_tracking_vehicle ( )

Definition at line 109 of file ctc.c.

References collective_tracking_control(), ctc_control, moving_target_px, moving_target_py, moving_target_vx, moving_target_vy, ctc_con::target_px, ctc_con::target_py, ctc_con::target_vx, and ctc_con::target_vy.

Here is the call graph for this function:

◆ collective_tracking_waypoint()

bool collective_tracking_waypoint ( uint8_t wp )

Definition at line 121 of file ctc.c.

References collective_tracking_control(), ctc_control, ctc_con::target_px, ctc_con::target_py, ctc_con::target_vx, ctc_con::target_vy, waypoints, point::x, and point::y.

Here is the call graph for this function:

◆ ctc_init()

void ctc_init ( void )

Definition at line 97 of file ctc.c.

References CTC_MAX_AC, DefaultPeriodic, register_periodic_telemetry(), send_ctc(), send_ctc_control(), and tableNei.

Here is the call graph for this function:

◆ send_ctc()

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

static

Definition at line 40 of file ctc.c.

References CTC_MAX_AC, dev, and tableNei.

Referenced by ctc_init().

Here is the caller graph for this function:

◆ send_ctc_control()

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

static

Definition at line 45 of file ctc.c.

References ctc_control, dev, ctc_con::ref_px, ctc_con::ref_py, ctc_con::target_px, ctc_con::target_py, ctc_con::target_vx, ctc_con::target_vy, ctc_con::v_centroid_x, and ctc_con::v_centroid_y.

Referenced by ctc_init().

Here is the caller graph for this function:

Variable Documentation

◆ before

uint32_t before = 0

Definition at line 84 of file ctc.c.

Referenced by collective_tracking_control().

◆ ctc_control

ctc_con ctc_control

Initial value:

= {CTC_GAIN_K1, CTC_GAIN_K2, CTC_GAIN_ALPHA, CTC_TIMEOUT,
                       0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, CTC_OMEGA, CTC_TIME_BROAD
                      }

Definition at line 77 of file ctc.c.

Referenced by collective_tracking_control(), collective_tracking_point(), collective_tracking_vehicle(), collective_tracking_waypoint(), and send_ctc_control().