diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 8b7127604d..4e13aa6f69 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -304,9 +304,7 @@ protected: virtual bool set_mode(uint8_t mode) = 0; void set_ekf_origin(const Location& loc); - virtual MAV_TYPE frame_type() const = 0; virtual MAV_MODE base_mode() const = 0; - virtual uint32_t custom_mode() const = 0; virtual MAV_STATE system_status() const = 0; virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; } @@ -602,9 +600,6 @@ private: // mavlink routing object static MAVLink_routing routing; - // pointer to static frsky_telem for queueing of text messages - static AP_Frsky_Telem *frsky_telemetry_p; - static const AP_SerialManager *serialmanager_p; struct pending_param_request { @@ -738,6 +733,9 @@ public: return _singleton; } + virtual uint32_t custom_mode() const = 0; + virtual MAV_TYPE frame_type() const = 0; + void send_text(MAV_SEVERITY severity, const char *fmt, ...); void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list); virtual void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text); @@ -764,17 +762,9 @@ public: _out_of_time = val; } - /* - set a frsky_telem pointer for queueing - */ - void register_frsky_telemetry_callback(AP_Frsky_Telem *frsky_telemetry) { - frsky_telemetry_p = frsky_telemetry; - } + // frsky backend + AP_Frsky_Telem frsky; - // static frsky_telem pointer to support queueing text messages - AP_Frsky_Telem *frsky_telemetry_p; - - // install an alternative protocol handler bool install_alternative_protocol(mavlink_channel_t chan, GCS_MAVLINK::protocol_handler_fn_t handler); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e4917fdd3a..6c8487e31e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1892,9 +1892,7 @@ void GCS::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const cha } // add statustext message to FrSky lib queue - if (frsky_telemetry_p != NULL) { - frsky_telemetry_p->queue_message(severity, text); - } + frsky.queue_message(severity, text); AP_Notify *notify = AP_Notify::get_singleton(); if (notify) { @@ -2017,6 +2015,8 @@ void GCS::setup_uarts(AP_SerialManager &serial_manager) for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) { chan(i).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i); } + + frsky.init(); } // report battery2 state @@ -2271,10 +2271,10 @@ void GCS_MAVLINK::send_heartbeat() const { mavlink_msg_heartbeat_send( chan, - frame_type(), + gcs().frame_type(), MAV_AUTOPILOT_ARDUPILOTMEGA, base_mode(), - custom_mode(), + gcs().custom_mode(), system_status()); } diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index 001db936cb..f89ee86091 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -28,9 +28,7 @@ protected: bool set_mode(uint8_t mode) override { return false; }; // dummy information: - MAV_TYPE frame_type() const override { return MAV_TYPE_FIXED_WING; } MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; } - uint32_t custom_mode() const override { return 3; } // magic number MAV_STATE system_status() const override { return MAV_STATE_CALIBRATING; } bool set_home_to_current_location(bool lock) override { return false; } @@ -56,4 +54,7 @@ class GCS_Dummy : public GCS void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text) { hal.console->printf("TOGCS: %s\n", text); } void update_sensor_status_flags(void) override {}; + + MAV_TYPE frame_type() const override { return MAV_TYPE_FIXED_WING; } + uint32_t custom_mode() const override { return 3; } // magic number }; diff --git a/libraries/GCS_MAVLink/examples/routing/routing.cpp b/libraries/GCS_MAVLink/examples/routing/routing.cpp index 38e10f32b7..8b6e5c3236 100644 --- a/libraries/GCS_MAVLink/examples/routing/routing.cpp +++ b/libraries/GCS_MAVLink/examples/routing/routing.cpp @@ -34,9 +34,7 @@ protected: bool set_mode(uint8_t mode) override { return false; }; // dummy information: - MAV_TYPE frame_type() const override { return MAV_TYPE_FIXED_WING; } MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; } - uint32_t custom_mode() const override { return 3; } // magic number MAV_STATE system_status() const override { return MAV_STATE_CALIBRATING; } void send_nav_controller_output() const override {}; void send_pid_tuning() override {};