Tracker: make set_home bool, do not save bad homes

This commit is contained in:
Peter Barker 2019-02-21 12:43:55 +11:00 committed by Peter Barker
parent ecbe67a0fe
commit 9f6027b332
5 changed files with 24 additions and 11 deletions

View File

@ -111,7 +111,9 @@ void Tracker::one_second_loop()
// set home to current location
Location temp_loc;
if (ahrs.get_location(temp_loc)) {
set_home(temp_loc);
if (!set_home(temp_loc)){
// fail silently
}
}
}

View File

@ -470,7 +470,10 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
// check if this is the HOME wp
if (packet.seq == 0) {
tracker.set_home(tell_command); // New home in EEPROM
if (!tracker.set_home(tell_command)) {
result = MAV_MISSION_ERROR;
goto mission_failed;
}
send_text(MAV_SEVERITY_INFO,"New HOME received");
waypoint_receiving = false;
}

View File

@ -251,8 +251,8 @@ private:
// system.cpp
void init_tracker();
bool get_home_eeprom(struct Location &loc);
void set_home_eeprom(struct Location temp);
void set_home(struct Location temp);
bool set_home_eeprom(const Location &temp) WARN_IF_UNUSED;
bool set_home(const Location &temp) WARN_IF_UNUSED;
void arm_servos();
void disarm_servos();
void prepare_servos();

View File

@ -71,7 +71,9 @@ void Tracker::update_GPS(void)
// Now have an initial GPS position
// use it as the HOME position in future startups
current_loc = gps.location();
set_home(current_loc);
if (!set_home(current_loc)) {
// silently ignored
}
if (g.compass_enabled) {
// Set compass declination automatically

View File

@ -135,7 +135,7 @@ bool Tracker::get_home_eeprom(struct Location &loc)
return true;
}
void Tracker::set_home_eeprom(struct Location temp)
bool Tracker::set_home_eeprom(const Location &temp)
{
wp_storage.write_byte(0, 0);
wp_storage.write_uint32(1, temp.alt);
@ -144,20 +144,26 @@ void Tracker::set_home_eeprom(struct Location temp)
// Now have a home location in EEPROM
g.command_total.set_and_save(1); // At most 1 entry for HOME
return true;
}
void Tracker::set_home(struct Location temp)
bool Tracker::set_home(const Location &temp)
{
set_home_eeprom(temp);
current_loc = temp;
// check EKF origin has been set
Location ekf_origin;
if (ahrs.get_origin(ekf_origin)) {
if (!ahrs.set_home(temp)) {
// ignore error silently
return false;
}
}
if (!set_home_eeprom(temp)) {
return false;
}
current_loc = temp;
return true;
}
void Tracker::arm_servos()