aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: make setting of home boolean in preparation for sanity checks

Browse Source
This commit is contained in:
Peter Barker 2018-05-30 09:47:50 +10:00 committed by Peter Barker
parent 2e403bfd52
commit b6bf8d8368
6 changed files with 37 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.cpp
View File

@ -382,7 +382,9 @@ void Plane::update_GPS_10Hz(void)
ground_start_count = 5;
} else {
set_home_persistently(gps.location());
if (!set_home_persistently(gps.location())) {
// silently ignore failure...
}
next_WP_loc = prev_WP_loc = home;

21
ArduPlane/GCS_Mavlink.cpp
View File

@ -748,7 +748,9 @@ void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status,
bool GCS_MAVLINK_Plane::set_home_to_current_location(bool lock)
{
plane.set_home_persistently(AP::gps().location());
if (!plane.set_home_persistently(AP::gps().location())) {
return false;
}
if (lock) {
AP::ahrs().lock_home();
}
@ -756,7 +758,9 @@ bool GCS_MAVLINK_Plane::set_home_to_current_location(bool lock)
}
bool GCS_MAVLINK_Plane::set_home(const Location& loc, bool lock)
{
AP::ahrs().set_home(loc);
if (!AP::ahrs().set_home(loc)) {
return false;
}
if (lock) {
AP::ahrs().lock_home();
}
@ -968,7 +972,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
// param6 : longitude
// param7 : altitude (absolute)
if (is_equal(packet.param1,1.0f)) {
plane.set_home_persistently(AP::gps().location());
if (!plane.set_home_persistently(AP::gps().location())) {
return MAV_RESULT_FAILED;
}
AP::ahrs().lock_home();
return MAV_RESULT_ACCEPTED;
} else {
@ -988,7 +994,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
set_home(new_home_loc, true);
if (!set_home(new_home_loc, true)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
}
break;
@ -1298,7 +1306,10 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
new_home_loc.lat = packet.latitude;
new_home_loc.lng = packet.longitude;
new_home_loc.alt = packet.altitude / 10;
set_home(new_home_loc, false);
if (!set_home(new_home_loc, false)) {
// silently fails...
break;
}
break;
}

4
ArduPlane/GCS_Mavlink.h
View File

@ -37,8 +37,8 @@ protected:
bool persist_streamrates() const override { return true; }
bool set_home_to_current_location(bool lock) override;
bool set_home(const Location& loc, bool lock) override;
bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
private:

2
ArduPlane/Plane.h
View File

@ -854,7 +854,7 @@ private:
void set_guided_WP(void);
void update_home();
// set home location and store it persistently:
void set_home_persistently(const Location &loc);
bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
void do_RTL(int32_t alt);
bool verify_takeoff();
bool verify_loiter_unlim(const AP_Mission::Mission_Command &cmd);

12
ArduPlane/commands.cpp
View File

@ -120,17 +120,23 @@ void Plane::update_home()
if (ahrs.home_is_set() && !ahrs.home_is_locked()) {
Location loc;
if(ahrs.get_position(loc)) {
AP::ahrs().set_home(loc);
if (!AP::ahrs().set_home(loc)) {
// silently fail
}
}
}
barometer.update_calibration();
ahrs.resetHeightDatum();
}
void Plane::set_home_persistently(const Location &loc)
bool Plane::set_home_persistently(const Location &loc)
{
AP::ahrs().set_home(loc);
if (!AP::ahrs().set_home(loc)) {
return false;
}
// Save Home to EEPROM
mission.write_home_to_storage();
return true;
}

9
ArduPlane/commands_logic.cpp
View File

@ -901,10 +901,13 @@ void Plane::do_change_speed(const AP_Mission::Mission_Command& cmd)
void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 == 1 && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
set_home_persistently(gps.location());
if (!set_home_persistently(gps.location())) {
// silently ignore error
}
} else {
AP::ahrs().set_home(cmd.content.location);
gcs().send_ekf_origin();
if (AP::ahrs().set_home(cmd.content.location)) {
gcs().send_ekf_origin();
}
}
}