diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index f6abd52f1e..bebe4a9da2 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -99,6 +99,7 @@ #include "config.h" #include "GCS_Mavlink.h" +#include "GCS_Copter.h" #include "AP_Rally.h" // Rally point library #include "AP_Arming.h" @@ -136,6 +137,7 @@ class Copter : public AP_HAL::HAL::Callbacks { public: friend class GCS_MAVLINK_Copter; + friend class GCS_Copter; friend class AP_Rally_Copter; friend class Parameters; friend class ParametersG2; @@ -245,11 +247,8 @@ private: // GCS selection AP_SerialManager serial_manager; - static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; - - GCS_MAVLINK_Copter gcs_chan[MAVLINK_COMM_NUM_BUFFERS]; - GCS _gcs; // avoid using this; use gcs() - GCS &gcs() { return _gcs; } + GCS_Copter _gcs; // avoid using this; use gcs() + GCS_Copter &gcs() { return _gcs; } // User variables #ifdef USERHOOK_VARIABLES diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp new file mode 100644 index 0000000000..989bb5450f --- /dev/null +++ b/ArduCopter/GCS_Copter.cpp @@ -0,0 +1,15 @@ +#include "GCS_Copter.h" +#include "Copter.h" + +bool GCS_Copter::cli_enabled() const +{ +#if CLI_ENABLED == ENABLED + return copter.g.cli_enabled; +#else + return false; +#endif +} + +AP_HAL::BetterStream* GCS_Copter::cliSerial() { + return copter.cliSerial; +} diff --git a/ArduCopter/GCS_Copter.h b/ArduCopter/GCS_Copter.h new file mode 100644 index 0000000000..6cb03557a7 --- /dev/null +++ b/ArduCopter/GCS_Copter.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include "GCS_Mavlink.h" + +class GCS_Copter : public GCS +{ + friend class Copter; // for access to _chan in parameter declarations + +public: + + // return the number of valid GCS objects + uint8_t num_gcs() const override { return ARRAY_SIZE(_chan); }; + + // return GCS link at offset ofs + GCS_MAVLINK_Copter &chan(const uint8_t ofs) override { + return _chan[ofs]; + }; + +private: + + GCS_MAVLINK_Copter _chan[MAVLINK_COMM_NUM_BUFFERS]; + + bool cli_enabled() const override; + AP_HAL::BetterStream* cliSerial() override; + +}; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index f65439bb55..078628bd67 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -78,7 +78,7 @@ NOINLINE void Copter::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(get_frame_mav_type(), + gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(get_frame_mav_type(), base_mode, custom_mode, system_status); @@ -2018,7 +2018,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) void Copter::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; DataFlash.EnableWrites(false); @@ -2051,11 +2051,7 @@ void Copter::mavlink_delay_cb() */ void Copter::gcs_send_message(enum ap_message id) { - for (uint8_t i=0; iget_control_in() != 0) { - gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Throttle not zero"); + gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Throttle not zero"); ap.compass_mot = false; return MAV_RESULT_TEMPORARILY_REJECTED; } // check we are landed if (!ap.land_complete) { - gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Not landed"); + gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Not landed"); ap.compass_mot = false; return MAV_RESULT_TEMPORARILY_REJECTED; } @@ -98,13 +100,13 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan) AP_Notify::flags.esc_calibration = true; // warn user we are starting calibration - gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Starting calibration"); + gcs_chan.send_text(MAV_SEVERITY_INFO, "Starting calibration"); // inform what type of compensation we are attempting if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) { - gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Current"); + gcs_chan.send_text(MAV_SEVERITY_INFO, "Current"); } else{ - gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Throttle"); + gcs_chan.send_text(MAV_SEVERITY_INFO, "Throttle"); } // disable throttle and battery failsafe @@ -250,10 +252,10 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan) } compass.save_motor_compensation(); // display success message - gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Calibration successful"); + gcs_chan.send_text(MAV_SEVERITY_INFO, "Calibration successful"); } else { // compensation vector never updated, report failure - gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_NOTICE, "Failed"); + gcs_chan.send_text(MAV_SEVERITY_NOTICE, "Failed"); compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); } diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 3793e6bcda..ff587aaa1d 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -72,28 +72,30 @@ void Copter::motor_test_output() // return true if tests can continue, false if not bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc) { + GCS_MAVLINK_Copter &gcs_chan = gcs().chan(chan-MAVLINK_COMM_0); + // check board has initialised if (!ap.initialised) { - gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Board initialising"); + gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Board initialising"); return false; } // check rc has been calibrated arming.pre_arm_rc_checks(true); if(check_rc && !ap.pre_arm_rc_check) { - gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated"); + gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated"); return false; } // ensure we are landed if (!ap.land_complete) { - gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: vehicle not landed"); + gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: vehicle not landed"); return false; } // check if safety switch has been pushed if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { - gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Safety switch"); + gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Safety switch"); return false; } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index cbbd94adb9..b8f809c2b5 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -120,7 +120,15 @@ void Copter::init_ardupilot() serial_manager.init(); // setup first port early to allow BoardConfig to report errors - gcs_chan[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); + gcs().chan(0).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); + + +#if CLI_ENABLED == ENABLED + // specify callback function for CLI menu system + if (g.cli_enabled) { + gcs().set_run_cli_func(FUNCTOR_BIND_MEMBER(&Copter::run_cli, void, AP_HAL::UARTDriver *)); + } +#endif // Register mavlink_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay @@ -154,9 +162,7 @@ void Copter::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); #if FRSKY_TELEM_ENABLED == ENABLED // setup frsky, and pass a number of parameters to the library @@ -249,16 +255,7 @@ void Copter::init_ardupilot() #endif #if CLI_ENABLED == ENABLED - if (g.cli_enabled) { - 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 #if HIL_MODE != HIL_MODE_DISABLED