Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: factor out a Plane::set_home

This commit is contained in:
Peter Barker 2018-05-18 13:06:46 +10:00 committed by Andrew Tridgell
parent 69d8980608
commit 22306c370b
5 changed files with 32 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/ArduPlane.cpp
View File

@ -436,7 +436,11 @@ void Plane::update_GPS_10Hz(void)
ground_start_count = 5;
} else {
init_home();
gcs().send_text(MAV_SEVERITY_INFO, "Init HOME");
set_home_persistently(gps.location());
next_WP_loc = prev_WP_loc = home;
// set system clock for log timestamps
uint64_t gps_timestamp = gps.time_epoch_usec();

16
ArduPlane/GCS_Mavlink.cpp
View File

@ -776,7 +776,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
case MAV_CMD_DO_SET_HOME: {
result = MAV_RESULT_FAILED; // assume failure
if (is_equal(packet.param1, 1.0f)) {
plane.init_home();
plane.set_home_persistently(AP::gps().location());
} else {
// ensure param1 is zero
if (!is_zero(packet.param1)) {
@ -809,9 +809,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
}
new_home_loc.alt += plane.ahrs.get_home().alt;
}
plane.ahrs.set_home(new_home_loc);
AP::ahrs().Log_Write_Home_And_Origin();
gcs().send_home();
plane.set_home(new_home_loc);
result = MAV_RESULT_ACCEPTED;
gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
(double)(new_home_loc.lat*1.0e-7f),
@ -1076,7 +1074,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
// param7 : altitude (absolute)
result = MAV_RESULT_FAILED; // assume failure
if (is_equal(packet.param1,1.0f)) {
plane.init_home();
plane.set_home_persistently(AP::gps().location());
} else {
// ensure param1 is zero
if (!is_zero(packet.param1)) {
@ -1094,9 +1092,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
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);
plane.ahrs.set_home(new_home_loc);
AP::ahrs().Log_Write_Home_And_Origin();
gcs().send_home();
plane.set_home(new_home_loc);
result = MAV_RESULT_ACCEPTED;
gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
(double)(new_home_loc.lat*1.0e-7f),
@ -1511,9 +1507,7 @@ 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;
plane.ahrs.set_home(new_home_loc);
plane.ahrs.Log_Write_Home_And_Origin();
gcs().send_home();
plane.set_home(new_home_loc);
gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
(double)(new_home_loc.lat*1.0e-7f),
(double)(new_home_loc.lng*1.0e-7f),

5
ArduPlane/Plane.h
View File

@ -828,8 +828,11 @@ private:
void rangefinder_height_update(void);
void set_next_WP(const struct Location &loc);
void set_guided_WP(void);
void init_home();
void update_home();
// set home location and store it persistently:
void set_home_persistently(const Location &loc);
// set home location:
void set_home(const Location &loc);
void do_RTL(int32_t alt);
bool verify_takeoff();
bool verify_loiter_unlim();

37
ArduPlane/commands.cpp
View File

@ -103,24 +103,6 @@ void Plane::set_guided_WP(void)
loiter_angle_reset();
}
// run this at setup on the ground
// -------------------------------
void Plane::init_home()
{
gcs().send_text(MAV_SEVERITY_INFO, "Init HOME");
ahrs.set_home(gps.location());
ahrs.Log_Write_Home_And_Origin();
gcs().send_home();
// Save Home to EEPROM
mission.write_home_to_storage();
// Save prev loc
// -------------
next_WP_loc = prev_WP_loc = home;
}
/*
update home location from GPS
this is called as long as we have 3D lock and the arming switch is
@ -140,10 +122,23 @@ void Plane::update_home()
if (ahrs.home_is_set() && !ahrs.home_is_locked()) {
Location loc;
if(ahrs.get_position(loc)) {
ahrs.set_home(loc);
ahrs.Log_Write_Home_And_Origin();
gcs().send_home();
plane.set_home(loc);
}
}
barometer.update_calibration();
}
void Plane::set_home_persistently(const Location &loc)
{
set_home(loc);
// Save Home to EEPROM
mission.write_home_to_storage();
}
void Plane::set_home(const Location &loc)
{
ahrs.set_home(loc);
ahrs.Log_Write_Home_And_Origin();
gcs().send_home();
}

6
ArduPlane/commands_logic.cpp
View File

@ -934,11 +934,9 @@ 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) {
init_home();
set_home_persistently(gps.location());
} else {
ahrs.set_home(cmd.content.location);
ahrs.Log_Write_Home_And_Origin();
gcs().send_home();
plane.set_home(cmd.content.location);
gcs().send_ekf_origin();
}
}