diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index af481f68d3..b40544c2e5 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -91,7 +91,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan) // indicate we have set a custom mode base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - gcs_chan[chan-MAVLINK_COMM_0].send_heartbeat(MAV_TYPE_FIXED_WING, + gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(MAV_TYPE_FIXED_WING, base_mode, custom_mode, system_status); @@ -2099,7 +2099,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) void Plane::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; - if (!gcs_chan[0].initialised || in_mavlink_delay) return; + if (!gcs().chan(0).initialised || in_mavlink_delay) return; in_mavlink_delay = true; @@ -2129,11 +2129,7 @@ void Plane::mavlink_delay_cb() */ void Plane::gcs_send_message(enum ap_message id) { - for (uint8_t i=0; iprintf("%s\n", msg); + if (_chan[1].initialised && (_chan[1].get_uart() != NULL)) { + _chan[1].get_uart()->printf("%s\n", msg); + } + if (num_gcs() > 2 && _chan[2].initialised && (_chan[2].get_uart() != NULL)) { + _chan[2].get_uart()->printf("%s\n", msg); + } + } +} diff --git a/ArduPlane/GCS_Plane.h b/ArduPlane/GCS_Plane.h new file mode 100644 index 0000000000..8b8706f752 --- /dev/null +++ b/ArduPlane/GCS_Plane.h @@ -0,0 +1,37 @@ +#pragma once + +#include +#include "GCS_Mavlink.h" + +class GCS_Plane : public GCS +{ + friend class Plane; // for temporary access to num_gcs and links + +public: + + FUNCTOR_TYPEDEF(run_cli_fn, void, AP_HAL::UARTDriver*); + + // return the number of valid GCS objects + uint8_t num_gcs() const { return _num_gcs; }; + + // return GCS link at offset ofs + GCS_MAVLINK_Plane &chan(const uint8_t ofs) { return _chan[ofs]; }; + + void reset_cli_timeout(); + void send_message(enum ap_message id); + void send_mission_item_reached_message(uint16_t mission_index); + void data_stream_send(); + void update(); + void send_airspeed_calibration(const Vector3f &vg); + + void set_run_cli_func(run_cli_fn run_cli) { _run_cli = run_cli; } + void setup_uarts(AP_SerialManager &serial_manager); + void handle_interactive_setup(); + +private: + + uint8_t _num_gcs = MAVLINK_COMM_NUM_BUFFERS; + GCS_MAVLINK_Plane _chan[MAVLINK_COMM_NUM_BUFFERS]; + run_cli_fn _run_cli; + +}; diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index b560ad095c..9ecf1986e2 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -542,9 +542,7 @@ void Plane::log_init(void) gcs_send_text(MAV_SEVERITY_INFO, "Preparing log system"); DataFlash.Prep(); gcs_send_text(MAV_SEVERITY_INFO, "Prepared log system"); - for (uint8_t i=0; i #include "GCS_Mavlink.h" +#include "GCS_Plane.h" #include "quadplane.h" #include "tuning.h" @@ -144,6 +145,7 @@ public: friend class AP_Tuning_Plane; friend class AP_AdvancedFailsafe_Plane; friend class AP_Avoidance_Plane; + friend class GCS_Plane; Plane(void); @@ -250,10 +252,8 @@ private: // GCS selection AP_SerialManager serial_manager; - const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; - GCS_MAVLINK_Plane gcs_chan[MAVLINK_COMM_NUM_BUFFERS]; - GCS _gcs; // avoid using this; use gcs() - GCS &gcs() { return _gcs; } + GCS_Plane _gcs; // avoid using this; use gcs() + GCS_Plane &gcs() { return _gcs; } // selected navigation controller AP_Navigation *nav_controller = &L1_controller; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index f18c00ec51..8bdef27dc7 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -122,7 +122,7 @@ void Plane::init_ardupilot() // initialise serial ports serial_manager.init(); - gcs_chan[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); + gcs().chan(0).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); // Register mavlink_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay @@ -161,9 +161,7 @@ void Plane::init_ardupilot() check_usb_mux(); // setup telem slots with serial ports - for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) { - gcs_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i); - } + gcs().setup_uarts(serial_manager); // setup frsky #if FRSKY_TELEM_ENABLED == ENABLED @@ -229,16 +227,7 @@ void Plane::init_ardupilot() hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); #if CLI_ENABLED == ENABLED - if (g.cli_enabled == 1) { - const char *msg = "\nPress ENTER 3 times to start interactive setup\n"; - cliSerial->printf("%s\n", msg); - if (gcs_chan[1].initialised && (gcs_chan[1].get_uart() != nullptr)) { - gcs_chan[1].get_uart()->printf("%s\n", msg); - } - if (num_gcs > 2 && gcs_chan[2].initialised && (gcs_chan[2].get_uart() != nullptr)) { - gcs_chan[2].get_uart()->printf("%s\n", msg); - } - } + gcs().handle_interactive_setup(); #endif // CLI_ENABLED init_capabilities(); @@ -571,8 +560,8 @@ void Plane::check_long_failsafe() (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) { failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI && - gcs_chan[0].last_radio_status_remrssi_ms != 0 && - (tnow - gcs_chan[0].last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) { + gcs().chan(0).last_radio_status_remrssi_ms != 0 && + (tnow - gcs().chan(0).last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) { failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); } } else {