Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 10:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

Sub: consolidate set_home functions

Browse Source 
added lock argument instead of having twice as many functions
no functional change
This commit is contained in:
Randy Mackay 2017-06-05 17:04:04 +09:00
parent cb76bd8f3d
commit 7bae493138
5 changed files with 19 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduSub/GCS_Mavlink.cpp
View File

@ -1138,7 +1138,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
// param7 : altitude (absolute)
result = MAV_RESULT_FAILED; // assume failure
if (is_equal(packet.param1,1.0f) || (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7))) {
if (sub.set_home_to_current_location_and_lock()) {
if (sub.set_home_to_current_location(true)) {
result = MAV_RESULT_ACCEPTED;
}
} else {
@ -1151,7 +1151,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
if (!sub.far_from_EKF_origin(new_home_loc)) {
if (sub.set_home_and_lock(new_home_loc)) {
if (sub.set_home(new_home_loc, true)) {
result = MAV_RESULT_ACCEPTED;
}
}
@ -1789,7 +1789,7 @@ void GCS_MAVLINK_Sub::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)) {
sub.set_home_to_current_location_and_lock();
sub.set_home_to_current_location(true);
} else {
// sanity check location
if (!check_latlng(packet.latitude, packet.longitude)) {
@ -1802,7 +1802,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
if (sub.far_from_EKF_origin(new_home_loc)) {
break;
}
sub.set_home_and_lock(new_home_loc);
sub.set_home(new_home_loc, true);
}
break;
}

6
ArduSub/Sub.h
View File

@ -525,10 +525,8 @@ private:
void userhook_SuperSlowLoop();
void update_home_from_EKF();
void set_home_to_current_location_inflight();
bool set_home_to_current_location();
bool set_home_to_current_location_and_lock();
bool set_home_and_lock(const Location& loc);
bool set_home(const Location& loc);
bool set_home_to_current_location(bool lock);
bool set_home(const Location& loc, bool lock);
bool far_from_EKF_origin(const Location& loc);
void set_system_time_from_GPS();
void exit_mission();

36
ArduSub/commands.cpp
View File

@ -20,7 +20,7 @@ void Sub::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();
set_home_to_current_location(false);
}
}
@ -32,12 +32,12 @@ void Sub::set_home_to_current_location_inflight()
if (inertial_nav.get_location(temp_loc)) {
const struct Location &ekf_origin = inertial_nav.get_origin();
temp_loc.alt = ekf_origin.alt;
set_home(temp_loc);
set_home(temp_loc, false);
}
}
// set_home_to_current_location - set home to current GPS location
bool Sub::set_home_to_current_location()
bool Sub::set_home_to_current_location(bool lock)
{
// get current location from EKF
Location temp_loc;
@ -48,28 +48,7 @@ bool Sub::set_home_to_current_location()
// This also ensures that mission items with relative altitude frame, are always
// relative to the water's surface, whether in a high elevation lake, or at sea level.
temp_loc.alt -= barometer.get_altitude() * 100.0f;
return set_home(temp_loc);
}
return false;
}
// set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved
bool Sub::set_home_to_current_location_and_lock()
{
if (set_home_to_current_location()) {
set_home_state(HOME_SET_AND_LOCKED);
return true;
}
return false;
}
// set_home_and_lock - sets ahrs home (used for RTL) to specified location and locks so it cannot be moved
// unless this function is called again
bool Sub::set_home_and_lock(const Location& loc)
{
if (set_home(loc)) {
set_home_state(HOME_SET_AND_LOCKED);
return true;
return set_home(temp_loc, lock);
}
return false;
}
@ -77,7 +56,7 @@ bool Sub::set_home_and_lock(const Location& loc)
// set_home - sets ahrs home (used for RTL) to specified location
// initialises inertial nav and compass on first call
// returns true if home location set successfully
bool Sub::set_home(const Location& loc)
bool Sub::set_home(const Location& loc, bool lock)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0) {
@ -107,6 +86,11 @@ bool Sub::set_home(const Location& loc)
}
}
// lock home position
if (lock) {
set_home_state(HOME_SET_AND_LOCKED);
}
// log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin();

4
ArduSub/commands_logic.cpp
View File

@ -840,10 +840,10 @@ void Sub::do_change_speed(const AP_Mission::Mission_Command& cmd)
void Sub::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();
set_home_to_current_location(false);
} else {
if (!far_from_EKF_origin(cmd.content.location)) {
set_home(cmd.content.location);
set_home(cmd.content.location, false);
}
}
}

2
ArduSub/motors.cpp
View File

@ -55,7 +55,7 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
// Log_Write_Event(DATA_EKF_ALT_RESET);
} else if (ap.home_state == HOME_SET_NOT_LOCKED) {
// Reset home position if it has already been set before (but not locked)
set_home_to_current_location();
set_home_to_current_location(false);
}
// enable gps velocity based centrefugal force compensation