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

Copter: set EKF origin from first do-set-home command

This commit is contained in:
Randy Mackay 2017-04-19 16:17:56 +09:00
parent b255c7b370
commit 8f43b60247
3 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/GCS_Mavlink.cpp
View File

@ -1176,10 +1176,8 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
if (!copter.far_from_EKF_origin(new_home_loc)) { if (copter.set_home_and_lock(new_home_loc)) {
if (copter.set_home_and_lock(new_home_loc)) { result = MAV_RESULT_ACCEPTED;
result = MAV_RESULT_ACCEPTED;
}
} }
} }
break; break;
@ -2019,9 +2017,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
new_home_loc.lat = packet.latitude; new_home_loc.lat = packet.latitude;
new_home_loc.lng = packet.longitude; new_home_loc.lng = packet.longitude;
new_home_loc.alt = packet.altitude / 10; new_home_loc.alt = packet.altitude / 10;
if (copter.far_from_EKF_origin(new_home_loc)) {
break;
}
copter.set_home_and_lock(new_home_loc); copter.set_home_and_lock(new_home_loc);
} }
break; break;

12
ArduCopter/commands.cpp
View File

@ -76,6 +76,18 @@ bool Copter::set_home(const Location& loc)
return false; return false;
} }
// set EKF origin to home if it hasn't been set yet and vehicle is disarmed
// this is required to allowing flying in AUTO and GUIDED with only an optical flow
Location ekf_origin;
if (!motors->armed() && !ahrs.get_origin(ekf_origin)) {
ahrs.set_origin(loc);
}
// check home is close to EKF origin
if (far_from_EKF_origin(loc)) {
return false;
}
// set ahrs home (used for RTL) // set ahrs home (used for RTL)
ahrs.set_home(loc); ahrs.set_home(loc);

4
ArduCopter/commands_logic.cpp
View File

@ -1104,9 +1104,7 @@ void Copter::do_set_home(const AP_Mission::Mission_Command& cmd)
if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) { if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
set_home_to_current_location(); set_home_to_current_location();
} else { } else {
if (!far_from_EKF_origin(cmd.content.location)) { set_home(cmd.content.location);
set_home(cmd.content.location);
}
} }
} }