aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: avoid Guided submode change unless can change Loc to Vec-from-origin

Browse Source 
ordering problem between changing the submode and setting a valid position
This commit is contained in:
Peter Barker 2024-05-21 18:48:15 +10:00 committed by Randy Mackay
parent 766d92faa6
commit fcebd0c8c2
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/mode_guided.cpp
View File

@ -456,6 +456,13 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
return true; return true;
} }
// set position target and zero velocity and acceleration
Vector3f pos_target_f;
bool terrain_alt;
if (!wp_nav->get_vector_NEU(dest_loc, pos_target_f, terrain_alt)) {
return false;
}
// if configured to use position controller for position control // if configured to use position controller for position control
// ensure we are in position control mode // ensure we are in position control mode
if (guided_mode != SubMode::Pos) { if (guided_mode != SubMode::Pos) {
@ -465,13 +472,6 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
// set yaw state // set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// set position target and zero velocity and acceleration
Vector3f pos_target_f;
bool terrain_alt;
if (!wp_nav->get_vector_NEU(dest_loc, pos_target_f, terrain_alt)) {
return false;
}
// initialise terrain following if needed // initialise terrain following if needed
if (terrain_alt) { if (terrain_alt) {
// get current alt above terrain // get current alt above terrain