From 1be52495f1493390cd2d282c3fcc1d0668ee26a4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 8 Jul 2017 14:46:24 +1000 Subject: [PATCH] Plane: eliminate global static GCS_MAVLINK::send_home_all --- ArduPlane/GCS_Mavlink.cpp | 4 ++-- ArduPlane/GCS_Plane.h | 3 +++ ArduPlane/commands.cpp | 4 ++-- ArduPlane/commands_logic.cpp | 2 +- 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index de5e3692db..2bbfc42e06 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1462,7 +1462,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) plane.ahrs.set_home(new_home_loc); plane.home_is_set = HOME_SET_NOT_LOCKED; plane.Log_Write_Home_And_Origin(); - GCS_MAVLINK::send_home_all(new_home_loc); + gcs().send_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), @@ -1953,7 +1953,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) plane.ahrs.set_home(new_home_loc); plane.home_is_set = HOME_SET_NOT_LOCKED; plane.Log_Write_Home_And_Origin(); - GCS_MAVLINK::send_home_all(new_home_loc); + gcs().send_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), diff --git a/ArduPlane/GCS_Plane.h b/ArduPlane/GCS_Plane.h index ea726770be..ac5292803c 100644 --- a/ArduPlane/GCS_Plane.h +++ b/ArduPlane/GCS_Plane.h @@ -17,6 +17,9 @@ public: GCS_MAVLINK_Plane &chan(const uint8_t ofs) override { return _chan[ofs]; }; + const GCS_MAVLINK_Plane &chan(const uint8_t ofs) const override { + return _chan[ofs]; + }; void send_airspeed_calibration(const Vector3f &vg); diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index c4b00b9a30..8ce051e84e 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -109,7 +109,7 @@ void Plane::init_home() ahrs.set_home(gps.location()); home_is_set = HOME_SET_NOT_LOCKED; Log_Write_Home_And_Origin(); - GCS_MAVLINK::send_home_all(gps.location()); + gcs().send_home(gps.location()); // Save Home to EEPROM mission.write_home_to_storage(); @@ -138,7 +138,7 @@ void Plane::update_home() if(ahrs.get_position(loc)) { ahrs.set_home(loc); Log_Write_Home_And_Origin(); - GCS_MAVLINK::send_home_all(loc); + gcs().send_home(loc); } } barometer.update_calibration(); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 999ece81d0..e99d13d7c4 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -905,7 +905,7 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd) ahrs.set_home(cmd.content.location); home_is_set = HOME_SET_NOT_LOCKED; Log_Write_Home_And_Origin(); - GCS_MAVLINK::send_home_all(cmd.content.location); + gcs().send_home(cmd.content.location); } }