AP_SmartRTL Class Reference

#include <AP_SmartRTL.h>

Collaboration diagram for AP_SmartRTL:

Classes
struct	dist_point
struct	prune_loop_t
struct	simplify_start_finish_t

Public Types
enum	ThoroughCleanupType { THOROUGH_CLEAN_DEFAULT = 0, THOROUGH_CLEAN_ALL, THOROUGH_CLEAN_SIMPLIFY_ONLY, THOROUGH_CLEAN_PRUNE_ONLY }

Public Member Functions
	AP_SmartRTL (bool example_mode=false)
void	init ()
bool	is_active () const
uint16_t	get_num_points () const
const Vector3f &	get_point (uint16_t index) const
bool	pop_point (Vector3f &point)
void	set_home (bool position_ok)
void	set_home (bool position_ok, const Vector3f &current_pos)
void	update (bool position_ok, bool save_position)
void	update (bool position_ok, const Vector3f &current_pos)
bool	request_thorough_cleanup (ThoroughCleanupType clean_type=THOROUGH_CLEAN_DEFAULT)
void	cancel_request_for_thorough_cleanup ()
void	run_background_cleanup ()

Static Public Attributes
static const struct AP_Param::GroupInfo	var_info []

Private Types
enum	SRTL_Actions {   SRTL_POINT_ADD, SRTL_POINT_PRUNE, SRTL_POINT_SIMPLIFY, SRTL_ADD_FAILED_NO_SEMAPHORE,   SRTL_ADD_FAILED_PATH_FULL, SRTL_POP_FAILED_NO_SEMAPHORE, SRTL_DEACTIVATED_INIT_FAILED, SRTL_DEACTIVATED_BAD_POSITION,   SRTL_DEACTIVATED_BAD_POSITION_TIMEOUT, SRTL_DEACTIVATED_PATH_FULL_TIMEOUT, SRTL_DEACTIVATED_PROGRAM_ERROR }

Private Member Functions
bool	add_point (const Vector3f &point)
void	routine_cleanup (uint16_t path_points_count, uint16_t path_points_complete_limit)
bool	thorough_cleanup (uint16_t path_points_count, ThoroughCleanupType clean_type)
void	detect_simplifications ()
void	detect_loops ()
void	restart_simplify_if_new_points (uint16_t path_points_count)
void	restart_pruning_if_new_points ()
void	restart_simplification (uint16_t path_points_count)
void	reset_simplification ()
void	restart_pruning (uint16_t path_points_count)
void	reset_pruning ()
void	remove_points_by_simplify_bitmask ()
bool	remove_points_by_loops (uint16_t num_points_to_remove)
bool	add_loop (uint16_t start_index, uint16_t end_index, const Vector3f &midpoint)
void	deactivate (SRTL_Actions action, const char *reason)
void	log_action (SRTL_Actions action, const Vector3f &point=Vector3f())
bool	loops_overlap (const prune_loop_t &loop1, const prune_loop_t &loop2) const

Static Private Member Functions
static dist_point	segment_segment_dist (const Vector3f &p1, const Vector3f &p2, const Vector3f &p3, const Vector3f &p4)

Private Attributes
AP_Float	_accuracy
AP_Int16	_points_max
bool	_active
bool	_example_mode
bool	_home_saved
uint32_t	_last_good_position_ms
uint32_t	_last_position_save_ms
uint32_t	_thorough_clean_request_ms
uint32_t	_thorough_clean_complete_ms
ThoroughCleanupType	_thorough_clean_type
Vector3f *	_path
uint16_t	_path_points_max
uint16_t	_path_points_count
uint16_t	_path_points_completed_limit
AP_HAL::Semaphore *	_path_sem
struct {
bool   complete
bool   removal_required
uint16_t   path_points_count
uint16_t   path_points_completed = SMARTRTL_POINTS_MAX
simplify_start_finish_t *   stack
uint16_t   stack_max
uint16_t   stack_count
Bitmask   bitmask = Bitmask(SMARTRTL_POINTS_MAX)
}	_simplify
struct {
bool   complete
uint16_t   path_points_count
uint16_t   path_points_completed
uint16_t   i
uint16_t   j
prune_loop_t *   loops
uint16_t   loops_max
uint16_t   loops_count
}	_prune

Detailed Description

Definition at line 28 of file AP_SmartRTL.h.

Member Enumeration Documentation

SRTL_Actions

enum AP_SmartRTL::SRTL_Actions

private

Enumerator
SRTL_POINT_ADD
SRTL_POINT_PRUNE
SRTL_POINT_SIMPLIFY
SRTL_ADD_FAILED_NO_SEMAPHORE
SRTL_ADD_FAILED_PATH_FULL
SRTL_POP_FAILED_NO_SEMAPHORE
SRTL_DEACTIVATED_INIT_FAILED
SRTL_DEACTIVATED_BAD_POSITION
SRTL_DEACTIVATED_BAD_POSITION_TIMEOUT
SRTL_DEACTIVATED_PATH_FULL_TIMEOUT
SRTL_DEACTIVATED_PROGRAM_ERROR

Definition at line 87 of file AP_SmartRTL.h.

ThoroughCleanupType

enum AP_SmartRTL::ThoroughCleanupType

Enumerator
THOROUGH_CLEAN_DEFAULT
THOROUGH_CLEAN_ALL
THOROUGH_CLEAN_SIMPLIFY_ONLY
THOROUGH_CLEAN_PRUNE_ONLY

Definition at line 62 of file AP_SmartRTL.h.

Constructor & Destructor Documentation

AP_SmartRTL()

AP_SmartRTL::AP_SmartRTL

(

bool

example_mode = false

)

Definition at line 75 of file AP_SmartRTL.cpp.

Here is the call graph for this function:

Member Function Documentation

add_loop()

bool AP_SmartRTL::add_loop ( uint16_t  start_index, uint16_t  end_index, const Vector3f &  midpoint  )

private

Definition at line 722 of file AP_SmartRTL.cpp.

Referenced by detect_loops().

Here is the call graph for this function:

Here is the caller graph for this function:

add_point()

bool AP_SmartRTL::add_point ( const Vector3f &  point )

private

Definition at line 287 of file AP_SmartRTL.cpp.

Referenced by set_home(), and update().

Here is the call graph for this function:

Here is the caller graph for this function:

cancel_request_for_thorough_cleanup()

void AP_SmartRTL::cancel_request_for_thorough_cleanup

(

)

Definition at line 277 of file AP_SmartRTL.cpp.

deactivate()

void AP_SmartRTL::deactivate ( SRTL_Actions  action, const char *  reason  )

private

Definition at line 823 of file AP_SmartRTL.cpp.

Referenced by remove_points_by_loops(), remove_points_by_simplify_bitmask(), and update().

Here is the call graph for this function:

Here is the caller graph for this function:

detect_loops()

void AP_SmartRTL::detect_loops ( )

private

This method runs for the allotted time, and detects loops in a path. Any detected loops are added to _prune.loops, this function does not alter the path in memory. It works by comparing the line segment between any two sequential points to the line segment between any other two sequential points. If they get close enough, anything between them could be pruned.

reset_pruning should have been called at least once before this function is called to setup the indexes (_prune.i, etc)

Definition at line 526 of file AP_SmartRTL.cpp.

Referenced by routine_cleanup(), and thorough_cleanup().

Here is the call graph for this function:

Here is the caller graph for this function:

detect_simplifications()

void AP_SmartRTL::detect_simplifications ( )

private

Definition at line 452 of file AP_SmartRTL.cpp.

Referenced by routine_cleanup(), and thorough_cleanup().

Here is the call graph for this function:

Here is the caller graph for this function:

get_num_points()

uint16_t AP_SmartRTL::get_num_points

(

)

const

Definition at line 133 of file AP_SmartRTL.cpp.

Referenced by check_path(), and is_active().

Here is the caller graph for this function:

get_point()

const Vector3f& AP_SmartRTL::get_point ( uint16_t  index ) const

inline

Definition at line 45 of file AP_SmartRTL.h.

Referenced by check_path().

Here is the call graph for this function:

Here is the caller graph for this function:

init()

void AP_SmartRTL::init

(

void

)

Definition at line 83 of file AP_SmartRTL.cpp.

Referenced by setup().

Here is the call graph for this function:

Here is the caller graph for this function:

is_active()

bool AP_SmartRTL::is_active ( ) const

inline

Definition at line 39 of file AP_SmartRTL.h.

Here is the call graph for this function:

log_action()

void AP_SmartRTL::log_action ( SRTL_Actions  action, const Vector3f &  point = Vector3f()  )

private

Definition at line 831 of file AP_SmartRTL.cpp.

Referenced by add_point(), deactivate(), init(), pop_point(), remove_points_by_loops(), and remove_points_by_simplify_bitmask().

Here is the call graph for this function:

Here is the caller graph for this function:

loops_overlap()

bool AP_SmartRTL::loops_overlap ( const prune_loop_t &  loop1, const prune_loop_t &  loop2  ) const

private

Definition at line 839 of file AP_SmartRTL.cpp.

Referenced by add_loop().

Here is the caller graph for this function:

pop_point()

bool AP_SmartRTL::pop_point

(

Vector3f &

point

)

Definition at line 139 of file AP_SmartRTL.cpp.

Referenced by get_point().

Here is the call graph for this function:

Here is the caller graph for this function:

remove_points_by_loops()

bool AP_SmartRTL::remove_points_by_loops ( uint16_t  num_points_to_remove )

private

Definition at line 661 of file AP_SmartRTL.cpp.

Referenced by routine_cleanup(), and thorough_cleanup().

Here is the call graph for this function:

Here is the caller graph for this function:

remove_points_by_simplify_bitmask()

void AP_SmartRTL::remove_points_by_simplify_bitmask ( )

private

Definition at line 623 of file AP_SmartRTL.cpp.

Referenced by routine_cleanup(), and thorough_cleanup().

Here is the call graph for this function:

Here is the caller graph for this function:

request_thorough_cleanup()

bool AP_SmartRTL::request_thorough_cleanup

(

ThoroughCleanupType

clean_type = THOROUGH_CLEAN_DEFAULT

)

Definition at line 251 of file AP_SmartRTL.cpp.

Referenced by loop().

Here is the call graph for this function:

Here is the caller graph for this function:

reset_pruning()

void AP_SmartRTL::reset_pruning ( )

private

Definition at line 615 of file AP_SmartRTL.cpp.

Referenced by set_home().

Here is the call graph for this function:

Here is the caller graph for this function:

reset_simplification()

void AP_SmartRTL::reset_simplification ( )

private

Definition at line 599 of file AP_SmartRTL.cpp.

Referenced by set_home().

Here is the call graph for this function:

Here is the caller graph for this function:

restart_pruning()

void AP_SmartRTL::restart_pruning ( uint16_t  path_points_count )

private

Definition at line 606 of file AP_SmartRTL.cpp.

Referenced by reset_pruning(), and restart_pruning_if_new_points().

Here is the caller graph for this function:

restart_pruning_if_new_points()

void AP_SmartRTL::restart_pruning_if_new_points ( )

private

Definition at line 580 of file AP_SmartRTL.cpp.

Referenced by routine_cleanup(), and thorough_cleanup().

Here is the call graph for this function:

Here is the caller graph for this function:

restart_simplification()

void AP_SmartRTL::restart_simplification ( uint16_t  path_points_count )

private

Definition at line 589 of file AP_SmartRTL.cpp.

Referenced by reset_simplification(), restart_simplify_if_new_points(), and routine_cleanup().

Here is the caller graph for this function:

restart_simplify_if_new_points()

void AP_SmartRTL::restart_simplify_if_new_points ( uint16_t  path_points_count )

private

Definition at line 571 of file AP_SmartRTL.cpp.

Referenced by thorough_cleanup().

Here is the call graph for this function:

Here is the caller graph for this function:

routine_cleanup()

void AP_SmartRTL::routine_cleanup ( uint16_t  path_points_count, uint16_t  path_points_complete_limit  )

private

Definition at line 361 of file AP_SmartRTL.cpp.

Referenced by run_background_cleanup().

Here is the call graph for this function:

Here is the caller graph for this function:

run_background_cleanup()

void AP_SmartRTL::run_background_cleanup

(

)

Definition at line 320 of file AP_SmartRTL.cpp.

Referenced by init(), and loop().

Here is the call graph for this function:

Here is the caller graph for this function:

segment_segment_dist()

AP_SmartRTL::dist_point AP_SmartRTL::segment_segment_dist ( const Vector3f &  p1, const Vector3f &  p2, const Vector3f &  p3, const Vector3f &  p4  )

staticprivate

Returns the closest distance in 3D space between any part of two input segments, defined from p1 to p2 and from p3 to p4. Also returns the point which is halfway between

Limitation: This function does not work for parallel lines. In this case, dist_point.distance will be FLT_MAX. This does not matter for the path cleanup algorithm because the pruning will still occur fine between the first parallel segment and a segment which is directly before or after the second segment.

Definition at line 785 of file AP_SmartRTL.cpp.

Referenced by detect_loops().

Here is the call graph for this function:

Here is the caller graph for this function:

set_home() [1/2]

void AP_SmartRTL::set_home

(

bool

position_ok

)

Definition at line 169 of file AP_SmartRTL.cpp.

Referenced by get_point(), reset(), and update().

Here is the call graph for this function:

Here is the caller graph for this function:

set_home() [2/2]

void AP_SmartRTL::set_home	(	bool	position_ok,
		const Vector3f &	current_pos
	)

Definition at line 176 of file AP_SmartRTL.cpp.

Here is the call graph for this function:

thorough_cleanup()

bool AP_SmartRTL::thorough_cleanup ( uint16_t  path_points_count, ThoroughCleanupType  clean_type  )

private

Definition at line 412 of file AP_SmartRTL.cpp.

Referenced by run_background_cleanup().

Here is the call graph for this function:

Here is the caller graph for this function:

update() [1/2]

void AP_SmartRTL::update	(	bool	position_ok,
		bool	save_position
	)

Definition at line 207 of file AP_SmartRTL.cpp.

Referenced by get_point(), and reset().

Here is the call graph for this function:

Here is the caller graph for this function:

update() [2/2]

void AP_SmartRTL::update	(	bool	position_ok,
		const Vector3f &	current_pos
	)

Definition at line 223 of file AP_SmartRTL.cpp.

Here is the call graph for this function:

Member Data Documentation

_accuracy

AP_Float AP_SmartRTL::_accuracy

private

Definition at line 169 of file AP_SmartRTL.h.

Referenced by add_point(), and init().

_active

bool AP_SmartRTL::_active

private

Definition at line 173 of file AP_SmartRTL.h.

Referenced by deactivate(), is_active(), log_action(), pop_point(), request_thorough_cleanup(), run_background_cleanup(), set_home(), and update().

_example_mode

bool AP_SmartRTL::_example_mode

private

Definition at line 174 of file AP_SmartRTL.h.

Referenced by init(), and log_action().

_home_saved

bool AP_SmartRTL::_home_saved

private

Definition at line 175 of file AP_SmartRTL.h.

Referenced by set_home(), and update().

_last_good_position_ms

uint32_t AP_SmartRTL::_last_good_position_ms

private

Definition at line 176 of file AP_SmartRTL.h.

Referenced by set_home(), and update().

_last_position_save_ms

uint32_t AP_SmartRTL::_last_position_save_ms

private

Definition at line 177 of file AP_SmartRTL.h.

Referenced by update().

_path

Vector3f* AP_SmartRTL::_path

private

Definition at line 183 of file AP_SmartRTL.h.

Referenced by add_loop(), add_point(), detect_loops(), detect_simplifications(), get_point(), init(), pop_point(), remove_points_by_loops(), remove_points_by_simplify_bitmask(), and set_home().

_path_points_completed_limit

uint16_t AP_SmartRTL::_path_points_completed_limit

private

Definition at line 186 of file AP_SmartRTL.h.

Referenced by pop_point(), and run_background_cleanup().

_path_points_count

uint16_t AP_SmartRTL::_path_points_count

private

Definition at line 185 of file AP_SmartRTL.h.

Referenced by add_point(), get_num_points(), log_action(), pop_point(), remove_points_by_loops(), remove_points_by_simplify_bitmask(), run_background_cleanup(), and set_home().

_path_points_max

uint16_t AP_SmartRTL::_path_points_max

private

Definition at line 184 of file AP_SmartRTL.h.

Referenced by add_point(), init(), log_action(), and routine_cleanup().

_path_sem

AP_HAL::Semaphore* AP_SmartRTL::_path_sem

private

Definition at line 187 of file AP_SmartRTL.h.

Referenced by add_point(), init(), pop_point(), remove_points_by_loops(), remove_points_by_simplify_bitmask(), and run_background_cleanup().

_points_max

AP_Int16 AP_SmartRTL::_points_max

private

Definition at line 170 of file AP_SmartRTL.h.

Referenced by init().

_prune

struct { ... } AP_SmartRTL::_prune

Referenced by add_loop(), detect_loops(), init(), remove_points_by_loops(), reset_pruning(), restart_pruning(), restart_pruning_if_new_points(), routine_cleanup(), and thorough_cleanup().

_simplify

struct { ... } AP_SmartRTL::_simplify

Referenced by AP_SmartRTL(), detect_simplifications(), init(), remove_points_by_simplify_bitmask(), reset_simplification(), restart_pruning_if_new_points(), restart_simplification(), restart_simplify_if_new_points(), routine_cleanup(), and thorough_cleanup().

_thorough_clean_complete_ms

uint32_t AP_SmartRTL::_thorough_clean_complete_ms

private

Definition at line 179 of file AP_SmartRTL.h.

Referenced by request_thorough_cleanup(), and run_background_cleanup().

_thorough_clean_request_ms

uint32_t AP_SmartRTL::_thorough_clean_request_ms

private

Definition at line 178 of file AP_SmartRTL.h.

Referenced by cancel_request_for_thorough_cleanup(), request_thorough_cleanup(), and run_background_cleanup().

_thorough_clean_type

ThoroughCleanupType AP_SmartRTL::_thorough_clean_type

private

Definition at line 180 of file AP_SmartRTL.h.

Referenced by request_thorough_cleanup(), and run_background_cleanup().

bitmask

Bitmask AP_SmartRTL::bitmask = Bitmask(SMARTRTL_POINTS_MAX)

Definition at line 203 of file AP_SmartRTL.h.

complete

bool AP_SmartRTL::complete

Definition at line 196 of file AP_SmartRTL.h.

i

uint16_t AP_SmartRTL::i

Definition at line 217 of file AP_SmartRTL.h.

Referenced by add_loop(), detect_simplifications(), and remove_points_by_loops().

j

uint16_t AP_SmartRTL::j

Definition at line 218 of file AP_SmartRTL.h.

loops

prune_loop_t* AP_SmartRTL::loops

Definition at line 219 of file AP_SmartRTL.h.

loops_count

uint16_t AP_SmartRTL::loops_count

Definition at line 221 of file AP_SmartRTL.h.

loops_max

uint16_t AP_SmartRTL::loops_max

Definition at line 220 of file AP_SmartRTL.h.

path_points_completed

uint16_t AP_SmartRTL::path_points_completed = SMARTRTL_POINTS_MAX

Definition at line 199 of file AP_SmartRTL.h.

path_points_count

uint16_t AP_SmartRTL::path_points_count

Definition at line 198 of file AP_SmartRTL.h.

Referenced by restart_pruning(), restart_simplification(), routine_cleanup(), and run_background_cleanup().

removal_required

bool AP_SmartRTL::removal_required

Definition at line 197 of file AP_SmartRTL.h.

stack

simplify_start_finish_t* AP_SmartRTL::stack

Definition at line 200 of file AP_SmartRTL.h.

stack_count

uint16_t AP_SmartRTL::stack_count

Definition at line 202 of file AP_SmartRTL.h.

stack_max

uint16_t AP_SmartRTL::stack_max

Definition at line 201 of file AP_SmartRTL.h.

var_info

const AP_Param::GroupInfo AP_SmartRTL::var_info

static

Initial value:

= {
    
    
    
    
    
    
    AP_GROUPINFO("ACCURACY", 0, AP_SmartRTL, _accuracy, SMARTRTL_ACCURACY_DEFAULT),

    
    
    
    
    
    
    AP_GROUPINFO("POINTS", 1, AP_SmartRTL, _points_max, SMARTRTL_POINTS_DEFAULT),

    AP_GROUPEND
}

Definition at line 82 of file AP_SmartRTL.h.

Referenced by AP_SmartRTL().

---

The documentation for this class was generated from the following files:

• libraries/AP_SmartRTL/AP_SmartRTL.h
• libraries/AP_SmartRTL/AP_SmartRTL.cpp