From cfeb449979f9293201b8627cc0d9b3e891b32435 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 13 Feb 2017 22:00:19 +1100 Subject: [PATCH] Rover: create GCS subclass, use inheritted methods --- APMrover2/GCS_Mavlink.cpp | 33 ++++++--------------------------- APMrover2/GCS_Rover.cpp | 15 +++++++++++++++ APMrover2/GCS_Rover.h | 25 +++++++++++++++++++++++++ APMrover2/Log.cpp | 4 +--- APMrover2/Parameters.cpp | 8 ++++---- APMrover2/Rover.cpp | 1 - APMrover2/Rover.h | 8 ++++---- APMrover2/system.cpp | 30 ++++++++++-------------------- 8 files changed, 65 insertions(+), 59 deletions(-) create mode 100644 APMrover2/GCS_Rover.cpp create mode 100644 APMrover2/GCS_Rover.h diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 4faa777d5b..9e3bb307cc 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -63,7 +63,7 @@ void Rover::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_GROUND_ROVER, + gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(MAV_TYPE_GROUND_ROVER, base_mode, custom_mode, system_status); @@ -1562,7 +1562,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) void Rover::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; - if (!gcs_chan[0].initialised || in_mavlink_delay) { + if (!gcs().chan(0).initialised || in_mavlink_delay) { return; } @@ -1597,11 +1597,7 @@ void Rover::mavlink_delay_cb() */ void Rover::gcs_send_message(enum ap_message id) { - for (uint8_t i=0; i < num_gcs; i++) { - if (gcs_chan[i].initialised) { - gcs_chan[i].send_message(id); - } - } + gcs().send_message(id); } /* @@ -1609,12 +1605,7 @@ void Rover::gcs_send_message(enum ap_message id) */ void Rover::gcs_send_mission_item_reached_message(uint16_t mission_index) { - for (uint8_t i=0; i < num_gcs; i++) { - if (gcs_chan[i].initialised) { - gcs_chan[i].mission_item_reached_index = mission_index; - gcs_chan[i].send_message(MSG_MISSION_ITEM_REACHED); - } - } + gcs().send_mission_item_reached_message(mission_index); } /* @@ -1622,11 +1613,7 @@ void Rover::gcs_send_mission_item_reached_message(uint16_t mission_index) */ void Rover::gcs_data_stream_send(void) { - for (uint8_t i=0; i < num_gcs; i++) { - if (gcs_chan[i].initialised) { - gcs_chan[i].data_stream_send(); - } - } + gcs().data_stream_send(); } /* @@ -1634,15 +1621,7 @@ void Rover::gcs_data_stream_send(void) */ void Rover::gcs_update(void) { - for (uint8_t i=0; i < num_gcs; i++) { - if (gcs_chan[i].initialised) { -#if CLI_ENABLED == ENABLED - gcs_chan[i].update(g.cli_enabled == 1 ? FUNCTOR_BIND_MEMBER(&Rover::run_cli, void, AP_HAL::UARTDriver *) : nullptr); -#else - gcs_chan[i].update(nullptr); -#endif - } - } + gcs().update(); } void Rover::gcs_send_text(MAV_SEVERITY severity, const char *str) diff --git a/APMrover2/GCS_Rover.cpp b/APMrover2/GCS_Rover.cpp new file mode 100644 index 0000000000..758cf5b704 --- /dev/null +++ b/APMrover2/GCS_Rover.cpp @@ -0,0 +1,15 @@ +#include "GCS_Rover.h" +#include "Rover.h" + +bool GCS_Rover::cli_enabled() const +{ +#if CLI_ENABLED == ENABLED + return rover.g.cli_enabled; +#else + return false; +#endif +} + +AP_HAL::BetterStream* GCS_Rover::cliSerial() { + return rover.cliSerial; +} diff --git a/APMrover2/GCS_Rover.h b/APMrover2/GCS_Rover.h new file mode 100644 index 0000000000..a66e2f6b40 --- /dev/null +++ b/APMrover2/GCS_Rover.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include "GCS_Mavlink.h" + +class GCS_Rover : public GCS +{ + friend class Rover; // 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_Rover &chan(const uint8_t ofs) override { return _chan[ofs]; }; + +private: + + GCS_MAVLINK_Rover _chan[MAVLINK_COMM_NUM_BUFFERS]; + + bool cli_enabled() const override; + AP_HAL::BetterStream* cliSerial() override; + +}; diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index e7d3828863..d0848e3ef2 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -473,9 +473,7 @@ void Rover::log_init(void) { DataFlash.Init(log_structure, ARRAY_SIZE(log_structure)); - for (uint8_t i=0; i < num_gcs; i++) { - gcs_chan[i].reset_cli_timeout(); - } + gcs().reset_cli_timeout(); if (g.log_bitmask != 0) { start_logging(); diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 69ce0ae7e1..aa29a5398e 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -402,19 +402,19 @@ const AP_Param::Info Rover::var_info[] = { // @Group: SR0_ // @Path: GCS_Mavlink.cpp - GOBJECTN(gcs_chan[0], gcs0, "SR0_", GCS_MAVLINK), + GOBJECTN(_gcs._chan[0], gcs0, "SR0_", GCS_MAVLINK), // @Group: SR1_ // @Path: GCS_Mavlink.cpp - GOBJECTN(gcs_chan[1], gcs1, "SR1_", GCS_MAVLINK), + GOBJECTN(_gcs._chan[1], gcs1, "SR1_", GCS_MAVLINK), // @Group: SR2_ // @Path: GCS_Mavlink.cpp - GOBJECTN(gcs_chan[2], gcs2, "SR2_", GCS_MAVLINK), + GOBJECTN(_gcs._chan[2], gcs2, "SR2_", GCS_MAVLINK), // @Group: SR3_ // @Path: GCS_Mavlink.cpp - GOBJECTN(gcs_chan[3], gcs3, "SR3_", GCS_MAVLINK), + GOBJECTN(_gcs._chan[3], gcs3, "SR3_", GCS_MAVLINK), // @Group: SERIAL // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp index 0151abf53f..01a3246888 100644 --- a/APMrover2/Rover.cpp +++ b/APMrover2/Rover.cpp @@ -33,7 +33,6 @@ Rover::Rover(void) : FUNCTOR_BIND_MEMBER(&Rover::start_command, bool, const AP_Mission::Mission_Command&), FUNCTOR_BIND_MEMBER(&Rover::verify_command_callback, bool, const AP_Mission::Mission_Command&), FUNCTOR_BIND_MEMBER(&Rover::exit_mission, void)), - num_gcs(MAVLINK_COMM_NUM_BUFFERS), ServoRelayEvents(relay), #if CAMERA == ENABLED camera(&relay), diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index a43e686aa2..b4176beae6 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -86,6 +86,7 @@ #endif #include "Parameters.h" #include "GCS_Mavlink.h" +#include "GCS_Rover.h" #include // ArduPilot Mega Declination Helper Library @@ -102,6 +103,7 @@ public: #if ADVANCED_FAILSAFE == ENABLED friend class AP_AdvancedFailsafe_Rover; #endif + friend class GCS_Rover; Rover(void); @@ -189,10 +191,8 @@ private: // GCS handling AP_SerialManager serial_manager; - const uint8_t num_gcs; - GCS_MAVLINK_Rover gcs_chan[MAVLINK_COMM_NUM_BUFFERS]; - GCS _gcs; // avoid using this; use gcs() - GCS &gcs() { return _gcs; } + GCS_Rover _gcs; // avoid using this; use gcs() + GCS_Rover &gcs() { return _gcs; } // relay support AP_Relay relay; diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 9539e80149..0d16c6cf88 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -100,12 +100,19 @@ void Rover::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); // Register mavlink_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); + // specify callback function for CLI menu system +#if CLI_ENABLED == ENABLED + if (gcs().cli_enabled()) { + gcs().set_run_cli_func(FUNCTOR_BIND_MEMBER(&Rover::run_cli, void, AP_HAL::UARTDriver *)); + } +#endif + BoardConfig.init(); #if HAL_WITH_UAVCAN BoardConfig_CAN.init(); @@ -135,9 +142,7 @@ void Rover::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 telemetry #if FRSKY_TELEM_ENABLED == ENABLED @@ -193,22 +198,7 @@ void Rover::init_ardupilot() #if CLI_ENABLED == ENABLED - // If the switch is in 'menu' mode, run the main menu. - // - // Since we can't be sure that the setup or test mode won't leave - // the system in an odd state, we don't let the user exit the top - // menu; they must reset in order to fly. - // - 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 init_capabilities();