mirror of https://github.com/ArduPilot/ardupilot
Copter: make setting of home boolean in preparation for sanity checks
This commit is contained in:
parent
b6bf8d8368
commit
5ae52038fa
|
@ -673,8 +673,8 @@ private:
|
||||||
// commands.cpp
|
// commands.cpp
|
||||||
void update_home_from_EKF();
|
void update_home_from_EKF();
|
||||||
void set_home_to_current_location_inflight();
|
void set_home_to_current_location_inflight();
|
||||||
bool set_home_to_current_location(bool lock);
|
bool set_home_to_current_location(bool lock) WARN_IF_UNUSED;
|
||||||
bool set_home(const Location& loc, bool lock);
|
bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED;
|
||||||
bool far_from_EKF_origin(const Location& loc);
|
bool far_from_EKF_origin(const Location& loc);
|
||||||
|
|
||||||
// compassmot.cpp
|
// compassmot.cpp
|
||||||
|
|
|
@ -1249,7 +1249,9 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
||||||
mavlink_set_home_position_t packet;
|
mavlink_set_home_position_t packet;
|
||||||
mavlink_msg_set_home_position_decode(msg, &packet);
|
mavlink_msg_set_home_position_decode(msg, &packet);
|
||||||
if((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) {
|
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 {
|
} else {
|
||||||
// sanity check location
|
// sanity check location
|
||||||
if (!check_latlng(packet.latitude, packet.longitude)) {
|
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.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;
|
||||||
copter.set_home(new_home_loc, true);
|
if (!copter.set_home(new_home_loc, true)) {
|
||||||
|
// silently ignored
|
||||||
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
|
@ -38,8 +38,8 @@ protected:
|
||||||
|
|
||||||
void handle_mount_message(const mavlink_message_t* msg) override;
|
void handle_mount_message(const mavlink_message_t* msg) override;
|
||||||
|
|
||||||
bool set_home_to_current_location(bool lock) override;
|
bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
|
||||||
bool set_home(const Location& loc, bool lock) override;
|
bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
|
|
@ -13,7 +13,9 @@ void Copter::update_home_from_EKF()
|
||||||
set_home_to_current_location_inflight();
|
set_home_to_current_location_inflight();
|
||||||
} else {
|
} else {
|
||||||
// move home to current ekf location (this will set home_state to HOME_SET)
|
// 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();
|
const bool home_was_set = ahrs.home_is_set();
|
||||||
|
|
||||||
// set ahrs home (used for RTL)
|
// set ahrs home (used for RTL)
|
||||||
ahrs.set_home(loc);
|
if (!ahrs.set_home(loc)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
// init inav and compass declination
|
// init inav and compass declination
|
||||||
if (!home_was_set) {
|
if (!home_was_set) {
|
||||||
|
|
|
@ -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)
|
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)) {
|
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 {
|
} else {
|
||||||
copter.set_home(cmd.content.location, false);
|
if (!copter.set_home(cmd.content.location, false)) {
|
||||||
|
// ignore failure
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -188,7 +188,9 @@ bool Copter::init_arm_motors(const AP_Arming::ArmingMethod method, const bool do
|
||||||
arming_altitude_m = 0;
|
arming_altitude_m = 0;
|
||||||
} else if (!ahrs.home_is_locked()) {
|
} else if (!ahrs.home_is_locked()) {
|
||||||
// Reset home position if it has already been set before (but not 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
|
// remember the height when we armed
|
||||||
arming_altitude_m = inertial_nav.get_altitude() * 0.01;
|
arming_altitude_m = inertial_nav.get_altitude() * 0.01;
|
||||||
|
|
Loading…
Reference in New Issue