Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

Copter: avoid working with uninitialised home location

Browse Source
This commit is contained in:
Peter Barker 2018-10-26 13:10:32 +11:00 committed by Randy Mackay
parent d34c4b01f4
commit 08a18d6a0a
3 changed files with 24 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/defines.h
View File

@ -160,6 +160,7 @@ enum GuidedMode {
// RTL states // RTL states
enum RTLState { enum RTLState {
RTL_Starting,
RTL_InitialClimb, RTL_InitialClimb,
RTL_ReturnHome, RTL_ReturnHome,
RTL_LoiterAtHome, RTL_LoiterAtHome,

7
ArduCopter/mode.h
View File

@ -1009,10 +1009,9 @@ private:
void climb_return_run(); void climb_return_run();
void loiterathome_start(); void loiterathome_start();
void loiterathome_run(); void loiterathome_run();
void build_path(bool terrain_following_allowed); void build_path();
void compute_return_target(bool terrain_following_allowed); void compute_return_target();
// RTL
RTLState _state = RTL_InitialClimb; // records state of rtl (initial climb, returning home, etc) RTLState _state = RTL_InitialClimb; // records state of rtl (initial climb, returning home, etc)
bool _state_complete = false; // set to true if the current state is completed bool _state_complete = false; // set to true if the current state is completed
@ -1029,7 +1028,7 @@ private:
// Loiter timer - Records how long we have been in loiter // Loiter timer - Records how long we have been in loiter
uint32_t _loiter_start_time; uint32_t _loiter_start_time;
bool path_built; bool terrain_following_allowed;
}; };

30
ArduCopter/mode_rtl.cpp
View File

@ -19,8 +19,9 @@ bool Copter::ModeRTL::init(bool ignore_checks)
} }
// initialise waypoint and spline controller // initialise waypoint and spline controller
wp_nav->wp_and_spline_init(); wp_nav->wp_and_spline_init();
path_built = false; _state = RTL_Starting;
climb_start(); _state_complete = true; // see run() method below
terrain_following_allowed = !copter.failsafe.terrain;
return true; return true;
} }
@ -29,8 +30,9 @@ void Copter::ModeRTL::restart_without_terrain()
{ {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL);
if (rtl_path.terrain_used) { if (rtl_path.terrain_used) {
build_path(false); terrain_following_allowed = false;
climb_start(); _state = RTL_Starting;
_state_complete = true;
gcs().send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing"); gcs().send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing");
} }
} }
@ -39,14 +41,17 @@ void Copter::ModeRTL::restart_without_terrain()
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeRTL::run(bool disarm_on_land) void Copter::ModeRTL::run(bool disarm_on_land)
{ {
if (!path_built) { if (!motors->armed()) {
path_built = true; return;
build_path(!copter.failsafe.terrain);
} }
// check if we need to move to next state // check if we need to move to next state
if (_state_complete) { if (_state_complete) {
switch (_state) { switch (_state) {
case RTL_Starting:
build_path();
climb_start();
break;
case RTL_InitialClimb: case RTL_InitialClimb:
return_start(); return_start();
break; break;
@ -72,6 +77,11 @@ void Copter::ModeRTL::run(bool disarm_on_land)
// call the correct run function // call the correct run function
switch (_state) { switch (_state) {
case RTL_Starting:
// should not be reached:
_state = RTL_InitialClimb;
FALLTHROUGH;
case RTL_InitialClimb: case RTL_InitialClimb:
climb_return_run(); climb_return_run();
break; break;
@ -385,7 +395,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
land_run_vertical_control(); land_run_vertical_control();
} }
void Copter::ModeRTL::build_path(bool terrain_following_allowed) void Copter::ModeRTL::build_path()
{ {
// origin point is our stopping point // origin point is our stopping point
Vector3f stopping_point; Vector3f stopping_point;
@ -395,7 +405,7 @@ void Copter::ModeRTL::build_path(bool terrain_following_allowed)
rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME); rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME);
// compute return target // compute return target
compute_return_target(terrain_following_allowed); compute_return_target();
// climb target is above our origin point at the return altitude // climb target is above our origin point at the return altitude
rtl_path.climb_target = Location(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame()); rtl_path.climb_target = Location(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame());
@ -410,7 +420,7 @@ void Copter::ModeRTL::build_path(bool terrain_following_allowed)
// compute the return target - home or rally point // compute the return target - home or rally point
// return altitude in cm above home at which vehicle should return home // return altitude in cm above home at which vehicle should return home
// return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set) // return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
void Copter::ModeRTL::compute_return_target(bool terrain_following_allowed) void Copter::ModeRTL::compute_return_target()
{ {
// set return target to nearest rally point or home position (Note: alt is absolute) // set return target to nearest rally point or home position (Note: alt is absolute)
#if AC_RALLY == ENABLED #if AC_RALLY == ENABLED