mirror of https://github.com/ArduPilot/ardupilot
Copter: set EKF origin from first do-set-home command
This commit is contained in:
parent
b255c7b370
commit
8f43b60247
|
@ -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.lng = (int32_t)(packet.param6 * 1.0e7f);
|
||||
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)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
if (copter.set_home_and_lock(new_home_loc)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -2019,9 +2017,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
|||
new_home_loc.lat = packet.latitude;
|
||||
new_home_loc.lng = packet.longitude;
|
||||
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);
|
||||
}
|
||||
break;
|
||||
|
|
|
@ -76,6 +76,18 @@ bool Copter::set_home(const Location& loc)
|
|||
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)
|
||||
ahrs.set_home(loc);
|
||||
|
||||
|
|
|
@ -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)) {
|
||||
set_home_to_current_location();
|
||||
} else {
|
||||
if (!far_from_EKF_origin(cmd.content.location)) {
|
||||
set_home(cmd.content.location);
|
||||
}
|
||||
set_home(cmd.content.location);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue