follow_me.c File Reference

#include "follow_me.h"
#include "modules/datalink/telemetry.h"
#include "generated/modules.h"
#include "generated/flight_plan.h"

Include dependency graph for follow_me.c:

Go to the source code of this file.

Macros
#define	FOLLOW_ME_DISTANCE   45
#define	FOLLOW_ME_HEIGHT   60
#define	FOLLOW_ME_GPS_TIMEOUT   5000
#define	FOLLOW_ME_GROUND_TIMEOUT   5000
#define	FOLLOW_ME_FILT   0.9
#define	FOLLOW_ME_MOVING_WPS

Functions
void	follow_me_init (void)
	init function More...
void	follow_me_periodic (void)
	periodic function More...
void	follow_me_parse_target_pos (uint8_t *buf)
	on receiving a TARGET_POS message More...
void	follow_me_set_wp (uint8_t wp_id, float speed)
	run function More...

Variables
float	follow_me_distance = FOLLOW_ME_DISTANCE
	distance from the ground gps More...
float	follow_me_height = FOLLOW_ME_HEIGHT
	height from the ground gps More...
float	follow_me_heading = 180.
	heading direction in which to hover from (automatically set if ground is exceeding speed) More...
float	follow_me_filt = FOLLOW_ME_FILT
	Follow me course sin/cos filter value (higher is harder filter) More...
float	follow_me_diag_speed = 1.0
	Diagonal speed for follow me. More...
float	follow_me_gps_delay = 200
	Follow me GPS delay from the relative positionb packet (in ms) More...
float	follow_me_datalink_delay = 600
	Follow me datalink delay from the ground GPS packet (in ms) More...
float	follow_me_advance_ms = 800
	Follow me waypoint advance time in ms (multiplied by the ground speed) More...
float	follow_me_min_dist = 1
	Follow me minimum distance in meters when trying to approach with a certain speed. More...
float	follow_me_min_height = 30
	Follow me minimum height in meters when approaching with a speed. More...
static uint32_t	ground_time_msec = 0
static bool	ground_set = false
static struct LlaCoor_i	ground_lla
static float	ground_speed
static float	ground_climb
static float	ground_course
static float	ground_heading
static uint8_t	moving_wps [] = {FOLLOW_ME_MOVING_WPS}
static uint8_t	moving_wps_cnt = 0
static struct EnuCoor_f	last_targetpos
static float	last_targetpos_heading
static bool	last_targetpos_valid = false

Detailed Description

Author

Freek van Tienen freek.nosp@m..v.t.nosp@m.ienen.nosp@m.@gma.nosp@m.il.co.nosp@m.m Control a rotorcraft to follow at a defined distance from the target

Definition in file follow_me.c.

Macro Definition Documentation

FOLLOW_ME_DISTANCE

#define FOLLOW_ME_DISTANCE   45

Definition at line 34 of file follow_me.c.

FOLLOW_ME_FILT

#define FOLLOW_ME_FILT   0.9

Definition at line 54 of file follow_me.c.

FOLLOW_ME_GPS_TIMEOUT

#define FOLLOW_ME_GPS_TIMEOUT   5000

Definition at line 44 of file follow_me.c.

FOLLOW_ME_GROUND_TIMEOUT

#define FOLLOW_ME_GROUND_TIMEOUT   5000

Definition at line 49 of file follow_me.c.

FOLLOW_ME_HEIGHT

#define FOLLOW_ME_HEIGHT   60

Definition at line 39 of file follow_me.c.

FOLLOW_ME_MOVING_WPS

#define FOLLOW_ME_MOVING_WPS

Definition at line 59 of file follow_me.c.

Function Documentation

follow_me_init()

void follow_me_init

(

void

)

init function

Definition at line 87 of file follow_me.c.

References ground_set, ground_time_msec, last_targetpos_valid, moving_wps, and moving_wps_cnt.

follow_me_parse_target_pos()

void follow_me_parse_target_pos

(

uint8_t *

buf

)

on receiving a TARGET_POS message

Definition at line 147 of file follow_me.c.

References LlaCoor_i::alt, get_sys_time_msec(), ground_climb, ground_course, ground_heading, ground_lla, ground_set, ground_speed, ground_time_msec, LlaCoor_i::lat, and LlaCoor_i::lon.

Here is the call graph for this function:

follow_me_periodic()

void follow_me_periodic

(

void

)

periodic function

Definition at line 95 of file follow_me.c.

References DefaultChannel, DefaultDevice, enu_of_lla_point_i(), ground_heading, ground_lla, ground_set, last_targetpos, last_targetpos_heading, last_targetpos_valid, moving_wps, moving_wps_cnt, stateGetNedOrigin_i(), VECT3_COPY, VECT3_DIFF, VECT3_FLOAT_OF_CM, waypoint_get_enu_f(), waypoint_set_enu(), waypoints, EnuCoor_f::x, point::x, EnuCoor_f::y, point::y, and EnuCoor_f::z.

Here is the call graph for this function:

follow_me_set_wp()

void follow_me_set_wp	(	uint8_t	wp_id,
		float	speed
	)

run function

should be called in a flight plan stay block using pre_call will only set the x and y position and not the height

ex: <block name="Track Object"> <stay wp="STDBY" pre_call="object_tracking_run(WP_STDBY)"> </block>

Definition at line 170 of file follow_me.c.

References DefaultChannel, DefaultDevice, ENU_OF_TO_NED, follow_me_advance_ms, follow_me_datalink_delay, FOLLOW_ME_DISTANCE, follow_me_distance, follow_me_filt, FOLLOW_ME_GROUND_TIMEOUT, follow_me_heading, FOLLOW_ME_HEIGHT, follow_me_height, follow_me_min_dist, follow_me_min_height, get_sys_time_msec(), ground_course, ground_heading, ground_lla, ground_set, ground_speed, ground_time_msec, State::ned_initialized_i, ned_of_lla_point_i(), state, stateGetNedOrigin_i(), waypoint_set_enu(), waypoints, NedCoor_f::x, NedCoor_i::x, point::x, NedCoor_f::y, NedCoor_i::y, point::y, NedCoor_f::z, and NedCoor_i::z.

Here is the call graph for this function:

Variable Documentation

follow_me_advance_ms

float follow_me_advance_ms = 800

Follow me waypoint advance time in ms (multiplied by the ground speed)

Definition at line 69 of file follow_me.c.

Referenced by follow_me_set_wp().

follow_me_datalink_delay

float follow_me_datalink_delay = 600

Follow me datalink delay from the ground GPS packet (in ms)

Definition at line 68 of file follow_me.c.

Referenced by follow_me_set_wp().

follow_me_diag_speed

float follow_me_diag_speed = 1.0

Diagonal speed for follow me.

Definition at line 66 of file follow_me.c.

follow_me_distance

float follow_me_distance = FOLLOW_ME_DISTANCE

distance from the ground gps

Definition at line 62 of file follow_me.c.

Referenced by follow_me_set_wp().

follow_me_filt

float follow_me_filt = FOLLOW_ME_FILT

Follow me course sin/cos filter value (higher is harder filter)

Definition at line 65 of file follow_me.c.

Referenced by follow_me_set_wp().

follow_me_gps_delay

float follow_me_gps_delay = 200

Follow me GPS delay from the relative positionb packet (in ms)

Definition at line 67 of file follow_me.c.

follow_me_heading

float follow_me_heading = 180.

heading direction in which to hover from (automatically set if ground is exceeding speed)

Definition at line 64 of file follow_me.c.

Referenced by follow_me_set_wp().

follow_me_height

float follow_me_height = FOLLOW_ME_HEIGHT

height from the ground gps

Definition at line 63 of file follow_me.c.

Referenced by follow_me_set_wp().

follow_me_min_dist

float follow_me_min_dist = 1

Follow me minimum distance in meters when trying to approach with a certain speed.

Definition at line 70 of file follow_me.c.

Referenced by follow_me_set_wp().

follow_me_min_height

float follow_me_min_height = 30

Follow me minimum height in meters when approaching with a speed.

Definition at line 71 of file follow_me.c.

Referenced by follow_me_set_wp().

ground_climb

float ground_climb

static

Definition at line 77 of file follow_me.c.

Referenced by follow_me_parse_target_pos().

ground_course

float ground_course

static

Definition at line 78 of file follow_me.c.

Referenced by follow_me_parse_target_pos(), and follow_me_set_wp().

ground_heading

float ground_heading

static

Definition at line 79 of file follow_me.c.

Referenced by follow_me_parse_target_pos(), follow_me_periodic(), and follow_me_set_wp().

ground_lla

struct LlaCoor_i ground_lla

static

Definition at line 74 of file follow_me.c.

Referenced by follow_me_parse_target_pos(), follow_me_periodic(), and follow_me_set_wp().

ground_set

bool ground_set = false

static

Definition at line 74 of file follow_me.c.

Referenced by follow_me_init(), follow_me_parse_target_pos(), follow_me_periodic(), and follow_me_set_wp().

ground_speed

float ground_speed

static

Definition at line 76 of file follow_me.c.

Referenced by follow_me_parse_target_pos(), follow_me_set_wp(), gvf_low_level_control_2D(), gvf_low_level_getState(), gvf_parametric_control_2D(), gvf_parametric_low_level_control_2D(), gvf_parametric_low_level_control_3D(), and snav_on_time().

ground_time_msec

uint32_t ground_time_msec = 0

static

Definition at line 73 of file follow_me.c.

Referenced by follow_me_init(), follow_me_parse_target_pos(), and follow_me_set_wp().

last_targetpos

struct EnuCoor_f last_targetpos

static

Definition at line 82 of file follow_me.c.

Referenced by follow_me_periodic().

last_targetpos_heading

float last_targetpos_heading

static

Definition at line 84 of file follow_me.c.

Referenced by follow_me_periodic().

last_targetpos_valid

bool last_targetpos_valid = false

static

Definition at line 85 of file follow_me.c.

Referenced by follow_me_init(), and follow_me_periodic().

moving_wps

uint8_t moving_wps[] = {FOLLOW_ME_MOVING_WPS}

static

Definition at line 81 of file follow_me.c.

Referenced by follow_me_init(), and follow_me_periodic().

moving_wps_cnt

uint8_t moving_wps_cnt = 0

static

Definition at line 82 of file follow_me.c.

Referenced by follow_me_init(), and follow_me_periodic().