Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rally: add RALLY_HOME_INC param to use Home as a Rally point

This commit is contained in:
KiwiHC16 2015-08-01 18:42:44 +02:00 committed by Randy Mackay
parent 407bb5933b
commit 7e99a052b8
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Rally/AP_Rally.cpp
View File

@ -25,6 +25,10 @@ StorageAccess AP_Rally::_storage(StorageManager::StorageRally);
#define RALLY_LIMIT_KM_DEFAULT 1.0f #define RALLY_LIMIT_KM_DEFAULT 1.0f
#endif #endif
#ifndef RALLY_HOME_INC_DEFAULT
#define RALLY_HOME_INC_DEFAULT 0
#endif
const AP_Param::GroupInfo AP_Rally::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_Rally::var_info[] PROGMEM = {
// @Param: TOTAL // @Param: TOTAL
// @DisplayName: Rally Total // @DisplayName: Rally Total
@ -40,6 +44,15 @@ const AP_Param::GroupInfo AP_Rally::var_info[] PROGMEM = {
// @Increment: 0.1 // @Increment: 0.1
AP_GROUPINFO("LIMIT_KM", 1, AP_Rally, _rally_limit_km, RALLY_LIMIT_KM_DEFAULT), AP_GROUPINFO("LIMIT_KM", 1, AP_Rally, _rally_limit_km, RALLY_LIMIT_KM_DEFAULT),
// @Param: HOME_INC
// @DisplayName: Rally Home Included
// @Description: Controls if Home has to be included as a Rally point (as a safe place) to RTL while a FS situation trigger a RTL. By default should be 0 as we don't know if Home is safe (e.g. boat launch...)
// @User: Standard
// @Units: na
// @Increment: na
// @Values: 0:Disabled,1:Enabled
AP_GROUPINFO("HOME_INC", 2, AP_Rally, _rally_home_inc, RALLY_HOME_INC_DEFAULT),
AP_GROUPEND AP_GROUPEND
}; };
@ -124,6 +137,11 @@ bool AP_Rally::find_nearest_rally_point(const Location &current_loc, RallyLocati
} }
} }
// Check if we should take Home in the Rally points
if ( _rally_home_inc && (get_distance(current_loc, home_loc)<min_dis) ) {
return false; // use home position
}
if ((_rally_limit_km > 0) && (min_dis > _rally_limit_km*1000.0f) && (get_distance(current_loc, home_loc) < min_dis)) { if ((_rally_limit_km > 0) && (min_dis > _rally_limit_km*1000.0f) && (get_distance(current_loc, home_loc) < min_dis)) {
return false; // use home position return false; // use home position
} }

1
libraries/AP_Rally/AP_Rally.h
View File

@ -71,6 +71,7 @@ private:
// parameters // parameters
AP_Int8 _rally_point_total_count; AP_Int8 _rally_point_total_count;
AP_Float _rally_limit_km; AP_Float _rally_limit_km;
AP_Int8 _rally_home_inc;
uint32_t _last_change_time_ms; uint32_t _last_change_time_ms;
}; };