Copter: make setting of home boolean in preparation for sanity checks

This commit is contained in:
Peter Barker 2018-05-30 10:47:08 +10:00 committed by Peter Barker
parent b6bf8d8368
commit 5ae52038fa
6 changed files with 25 additions and 11 deletions

View File

@ -673,8 +673,8 @@ private:
// commands.cpp
void update_home_from_EKF();
void set_home_to_current_location_inflight();
bool set_home_to_current_location(bool lock);
bool set_home(const Location& loc, bool lock);
bool set_home_to_current_location(bool lock) WARN_IF_UNUSED;
bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED;
bool far_from_EKF_origin(const Location& loc);
// compassmot.cpp

View File

@ -1249,7 +1249,9 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
mavlink_set_home_position_t packet;
mavlink_msg_set_home_position_decode(msg, &packet);
if((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) {
copter.set_home_to_current_location(true);
if (!copter.set_home_to_current_location(true)) {
// silently ignored
}
} else {
// sanity check location
if (!check_latlng(packet.latitude, packet.longitude)) {
@ -1259,7 +1261,9 @@ 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;
copter.set_home(new_home_loc, true);
if (!copter.set_home(new_home_loc, true)) {
// silently ignored
}
}
break;
}

View File

@ -38,8 +38,8 @@ protected:
void handle_mount_message(const mavlink_message_t* msg) override;
bool set_home_to_current_location(bool lock) override;
bool set_home(const Location& loc, bool lock) override;
bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
private:

View File

@ -13,7 +13,9 @@ void Copter::update_home_from_EKF()
set_home_to_current_location_inflight();
} else {
// move home to current ekf location (this will set home_state to HOME_SET)
set_home_to_current_location(false);
if (!set_home_to_current_location(false)) {
// ignore failure
}
}
}
@ -75,7 +77,9 @@ bool Copter::set_home(const Location& loc, bool lock)
const bool home_was_set = ahrs.home_is_set();
// set ahrs home (used for RTL)
ahrs.set_home(loc);
if (!ahrs.set_home(loc)) {
return false;
}
// init inav and compass declination
if (!home_was_set) {

View File

@ -1360,9 +1360,13 @@ void Copter::ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
void Copter::ModeAuto::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)) {
copter.set_home_to_current_location(false);
if (!copter.set_home_to_current_location(false)) {
// ignore failure
}
} else {
copter.set_home(cmd.content.location, false);
if (!copter.set_home(cmd.content.location, false)) {
// ignore failure
}
}
}

View File

@ -188,7 +188,9 @@ bool Copter::init_arm_motors(const AP_Arming::ArmingMethod method, const bool do
arming_altitude_m = 0;
} else if (!ahrs.home_is_locked()) {
// Reset home position if it has already been set before (but not locked)
set_home_to_current_location(false);
if (!set_home_to_current_location(false)) {
// ignore failure
}
// remember the height when we armed
arming_altitude_m = inertial_nav.get_altitude() * 0.01;