mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Rover: make setting of home boolean in preparation for sanity checks
This commit is contained in:
parent
f4097d254c
commit
a722fd1323
@ -166,7 +166,9 @@ void Rover::ahrs_update()
|
||||
|
||||
// set home from EKF if necessary and possible
|
||||
if (!ahrs.home_is_set()) {
|
||||
set_home_to_current_location(false);
|
||||
if (!set_home_to_current_location(false)) {
|
||||
// ignore this failure
|
||||
}
|
||||
}
|
||||
|
||||
// if using the EKF get a speed update now (from accelerometers)
|
||||
|
@ -93,7 +93,9 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
|
||||
// if disarmed clear mission and set home to current location
|
||||
if (!rover.arming.is_armed()) {
|
||||
rover.mode_auto.mission.clear();
|
||||
rover.set_home_to_current_location(false);
|
||||
if (!rover.set_home_to_current_location(false)) {
|
||||
// ignored
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -406,8 +406,8 @@ private:
|
||||
void update_mission(void);
|
||||
|
||||
// commands.cpp
|
||||
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;
|
||||
void update_home();
|
||||
|
||||
// compat.cpp
|
||||
|
@ -30,7 +30,9 @@ bool Rover::set_home(const Location& loc, bool lock)
|
||||
const bool home_was_set = ahrs.home_is_set();
|
||||
|
||||
// set ahrs home
|
||||
ahrs.set_home(loc);
|
||||
if (!ahrs.set_home(loc)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!home_was_set) {
|
||||
// log new home position which mission library will pull from ahrs
|
||||
@ -82,5 +84,7 @@ void Rover::update_home()
|
||||
return;
|
||||
}
|
||||
|
||||
ahrs.set_home(loc);
|
||||
if (!ahrs.set_home(loc)) {
|
||||
// silently ignored...
|
||||
}
|
||||
}
|
||||
|
@ -349,9 +349,13 @@ void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
|
||||
void ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if (cmd.p1 == 1 && rover.have_position) {
|
||||
rover.set_home_to_current_location(false);
|
||||
if (!rover.set_home_to_current_location(false)) {
|
||||
// ignored...
|
||||
}
|
||||
} else {
|
||||
rover.set_home(cmd.content.location, false);
|
||||
if (!rover.set_home(cmd.content.location, false)) {
|
||||
// ignored...
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user