mirror of https://github.com/ArduPilot/ardupilot
Tracker: make set_home bool, do not save bad homes
This commit is contained in:
parent
ecbe67a0fe
commit
9f6027b332
|
@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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()
|
||||
|
|
Loading…
Reference in New Issue