diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 8c5c644842..2e14edff3a 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "GCS_Mavlink.h" + #include "Plane.h" #include "version.h" @@ -636,7 +638,7 @@ bool Plane::telemetry_delayed(mavlink_channel_t chan) // try to send a message, return false if it won't fit in the serial tx buffer -bool GCS_MAVLINK::try_send_message(enum ap_message id) +bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) { if (plane.telemetry_delayed(chan)) { return false; @@ -975,7 +977,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { AP_GROUPEND }; -float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num) +float GCS_MAVLINK_Plane::adjust_rate_for_stream_trigger(enum streams stream_num) { // send at a much lower rate while handling waypoints and // parameter sends @@ -988,7 +990,7 @@ float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num) } void -GCS_MAVLINK::data_stream_send(void) +GCS_MAVLINK_Plane::data_stream_send(void) { plane.gcs_out_of_time = false; @@ -1110,7 +1112,7 @@ GCS_MAVLINK::data_stream_send(void) handle a request to switch to guided mode. This happens via a callback from handle_mission_item() */ -bool GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd) +bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd) { if (plane.control_mode != GUIDED) { // only accept position updates when in GUIDED mode @@ -1132,7 +1134,7 @@ bool GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd) handle a request to change current WP altitude. This happens via a callback from handle_mission_item() */ -void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd) +void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &cmd) { plane.next_WP_loc.alt = cmd.content.location.alt; if (cmd.content.location.flags.relative_alt) { @@ -1143,7 +1145,7 @@ void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd) plane.reset_offset_altitude(); } -void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) +void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h new file mode 100644 index 0000000000..453ed0b019 --- /dev/null +++ b/ArduPlane/GCS_Mavlink.h @@ -0,0 +1,22 @@ +#pragma once + +#include + +class GCS_MAVLINK_Plane : public GCS_MAVLINK +{ + +public: + + void data_stream_send(void) override; + +protected: + +private: + + float adjust_rate_for_stream_trigger(enum streams stream_num) override; + void handleMessage(mavlink_message_t * msg) override; + bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; + void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; + bool try_send_message(enum ap_message id) override; + +}; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c54a0ed968..0f4d000ff7 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -56,7 +56,6 @@ #include #include #include -#include #include // MAVLink GCS definitions #include // Serial manager library #include // Camera/Antenna mount @@ -90,6 +89,7 @@ #include #include +#include "GCS_Mavlink.h" #include "quadplane.h" #include "tuning.h" @@ -129,7 +129,7 @@ protected: */ class Plane : public AP_HAL::HAL::Callbacks { public: - friend class GCS_MAVLINK; + friend class GCS_MAVLINK_Plane; friend class Parameters; friend class AP_Arming_Plane; friend class QuadPlane; @@ -254,7 +254,7 @@ private: // GCS selection AP_SerialManager serial_manager; const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; - GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; + GCS_MAVLINK_Plane gcs[MAVLINK_COMM_NUM_BUFFERS]; // selected navigation controller AP_Navigation *nav_controller = &L1_controller;