diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 1bddf96f65..887a7485f1 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -52,10 +52,7 @@ void GCS::send_to_active_channels(uint32_t msgid, const char *pkt) return; } for (uint8_t i=0; i= num_gcs()) { return false; } - if (chan(c).alternative.handler && handler) { + if (chan(c)->alternative.handler && handler) { // already have one installed - we may need to add support for // multiple alternative handlers return false; } - chan(c).alternative.handler = handler; + chan(c)->alternative.handler = handler; return true; } diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 2a653a2986..e5954af1a3 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -37,6 +37,19 @@ } #define MAV_STREAM_TERMINATOR { (streams)0, nullptr, 0 } +#define GCS_MAVLINK_NUM_STREAM_RATES 10 +class GCS_MAVLINK_Parameters +{ +public: + + GCS_MAVLINK_Parameters(); + + static const struct AP_Param::GroupInfo var_info[]; + + // saveable rate of each stream + AP_Int16 streamRates[GCS_MAVLINK_NUM_STREAM_RATES]; +}; + /// /// @class GCS_MAVLINK /// @brief MAVLink transport control class @@ -45,11 +58,13 @@ class GCS_MAVLINK { public: friend class GCS; - GCS_MAVLINK(); + + GCS_MAVLINK(GCS_MAVLINK_Parameters ¶meters, AP_HAL::UARTDriver &uart); + virtual ~GCS_MAVLINK() {} + void update_receive(uint32_t max_time_us=1000); void update_send(); - void init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan); - void setup_uart(uint8_t instance); + bool init(uint8_t instance); void send_message(enum ap_message id); void send_text(MAV_SEVERITY severity, const char *fmt, ...) const FMT_PRINTF(3, 4); void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list) const; @@ -101,11 +116,6 @@ public: virtual uint8_t sysid_my_gcs() const = 0; virtual bool sysid_enforce() const { return false; } - static const struct AP_Param::GroupInfo var_info[]; - - // set to true if this GCS link is active - bool initialised; - // NOTE! The streams enum below and the // set of AP_Int16 stream rates _must_ be // kept in the same order @@ -123,6 +133,12 @@ public: NUM_STREAMS }; + // streams must be moved out into the top level for + // GCS_MAVLINK_Parameters to be able to use it. This is an + // extensive change, so we 'll just keep them in sync with a + // static assert for now: + static_assert(NUM_STREAMS == GCS_MAVLINK_NUM_STREAM_RATES, "num streams must equal num stream rates"); + bool is_high_bandwidth() { return chan == MAVLINK_COMM_0; } // return true if this channel has hardware flow control bool have_flow_control(); @@ -280,7 +296,7 @@ protected: uint8_t packet_overhead(void) const { return packet_overhead_chan(chan); } // saveable rate of each stream - AP_Int16 streamRates[NUM_STREAMS]; + AP_Int16 *streamRates; virtual bool persist_streamrates() const { return false; } void handle_request_data_stream(const mavlink_message_t &msg); @@ -701,9 +717,10 @@ public: 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); void service_statustext(void); - virtual GCS_MAVLINK &chan(const uint8_t ofs) = 0; - virtual const GCS_MAVLINK &chan(const uint8_t ofs) const = 0; - virtual uint8_t num_gcs() const = 0; + virtual GCS_MAVLINK *chan(const uint8_t ofs) = 0; + virtual const GCS_MAVLINK *chan(const uint8_t ofs) const = 0; + // return the number of valid GCS objects + uint8_t num_gcs() const { return _num_gcs; }; void send_message(enum ap_message id); void send_mission_item_reached_message(uint16_t mission_index); void send_named_float(const char *name, float value) const; @@ -719,7 +736,6 @@ public: void update_send(); void update_receive(); - virtual void setup_uarts(); // minimum amount of time (in microseconds) that must remain in // the main scheduler loop before we are allowed to send any @@ -728,6 +744,10 @@ public: virtual uint16_t min_loop_time_remaining_for_message_send_us() const { return 200; } + + void setup_console(); + void setup_uarts(); + bool out_of_time() const; // frsky backend @@ -740,7 +760,7 @@ public: bool install_alternative_protocol(mavlink_channel_t chan, GCS_MAVLINK::protocol_handler_fn_t handler); // get the VFR_HUD throttle - int16_t get_hud_throttle(void) const { return num_gcs()>0?chan(0).vfr_hud_throttle():0; } + int16_t get_hud_throttle(void) const { return num_gcs()>0?chan(0)->vfr_hud_throttle():0; } // update uart pass-thru void update_passthru(); @@ -753,16 +773,26 @@ public: protected: + virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, + AP_HAL::UARTDriver &uart) = 0; + uint32_t control_sensors_present; uint32_t control_sensors_enabled; uint32_t control_sensors_health; void update_sensor_status_flags(); virtual void update_vehicle_sensor_status_flags() {} + GCS_MAVLINK_Parameters chan_parameters[MAVLINK_COMM_NUM_BUFFERS]; + uint8_t _num_gcs; + GCS_MAVLINK *_chan[MAVLINK_COMM_NUM_BUFFERS]; + private: static GCS *_singleton; + void create_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, + AP_HAL::UARTDriver &uart); + struct statustext_t { uint8_t bitmask; mavlink_statustext_t msg; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 87b1b55d34..d909a2fe09 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -76,55 +76,29 @@ uint8_t GCS_MAVLINK::mavlink_private = 0; GCS *GCS::_singleton = nullptr; -GCS_MAVLINK::GCS_MAVLINK() +GCS_MAVLINK::GCS_MAVLINK(GCS_MAVLINK_Parameters ¶meters, + AP_HAL::UARTDriver &uart) { - AP_Param::setup_object_defaults(this, var_info); + _port = &uart; + + streamRates = parameters.streamRates; } -void -GCS_MAVLINK::init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan) -{ - if (!valid_channel(mav_chan)) { - return; - } - - _port = port; - chan = mav_chan; - - mavlink_comm_port[chan] = _port; - _queued_parameter = nullptr; - - snprintf(_perf_packet_name, sizeof(_perf_packet_name), "GCS_Packet_%u", chan); - _perf_packet = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_packet_name); - - snprintf(_perf_update_name, sizeof(_perf_update_name), "GCS_Update_%u", chan); - _perf_update = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_update_name); - - initialised = true; -} - - -/* - setup a UART, handling begin() and init() - */ -void -GCS_MAVLINK::setup_uart(uint8_t instance) +bool GCS_MAVLINK::init(uint8_t instance) { // search for serial port const AP_SerialManager& serial_manager = AP::serialmanager(); + const AP_SerialManager::SerialProtocol protocol = AP_SerialManager::SerialProtocol_MAVLink; - AP_HAL::UARTDriver *uart = serial_manager.find_serial(protocol, instance); - if (uart == nullptr) { - // return immediately if not found - return; - } - // get associated mavlink channel - mavlink_channel_t mav_chan; - if (!serial_manager.get_mavlink_channel(protocol, instance, mav_chan)) { + if (!serial_manager.get_mavlink_channel(protocol, instance, chan)) { // return immediately in unlikely case mavlink channel cannot be found - return; + return false; + } + // and init the gcs instance + if (!valid_channel(chan)) { + return false; } /* @@ -135,29 +109,35 @@ GCS_MAVLINK::setup_uart(uint8_t instance) 0x20 at 115200 on startup, which tells the bootloader to reset and boot normally */ - uart->begin(115200); - AP_HAL::UARTDriver::flow_control old_flow_control = uart->get_flow_control(); - uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); + _port->begin(115200); + AP_HAL::UARTDriver::flow_control old_flow_control = _port->get_flow_control(); + _port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); for (uint8_t i=0; i<3; i++) { hal.scheduler->delay(1); - uart->write(0x30); - uart->write(0x20); + _port->write(0x30); + _port->write(0x20); } // since tcdrain() and TCSADRAIN may not be implemented... hal.scheduler->delay(1); - uart->set_flow_control(old_flow_control); + _port->set_flow_control(old_flow_control); // now change back to desired baudrate - uart->begin(serial_manager.find_baudrate(protocol, instance)); + _port->begin(serial_manager.find_baudrate(protocol, instance)); - // and init the gcs instance - init(uart, mav_chan); + mavlink_comm_port[chan] = _port; - AP_SerialManager::SerialProtocol mavlink_protocol = AP::serialmanager().get_mavlink_protocol(mav_chan); + // create performance counters + snprintf(_perf_packet_name, sizeof(_perf_packet_name), "GCS_Packet_%u", chan); + _perf_packet = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_packet_name); + + snprintf(_perf_update_name, sizeof(_perf_update_name), "GCS_Update_%u", chan); + _perf_update = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_update_name); + + AP_SerialManager::SerialProtocol mavlink_protocol = serial_manager.get_mavlink_protocol(chan); mavlink_status_t *status = mavlink_get_channel_status(chan); if (status == nullptr) { - return; + return false; } if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2) { @@ -178,6 +158,8 @@ GCS_MAVLINK::setup_uart(uint8_t instance) // after experiments with MAVLink2 status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; } + + return true; } void GCS_MAVLINK::send_meminfo(void) @@ -1845,9 +1827,7 @@ void GCS::service_statustext(void) void GCS::send_message(enum ap_message id) { for (uint8_t i=0; isend_message(id); } } @@ -1872,9 +1852,7 @@ void GCS::update_send() _missionitemprotocol_rally->update(); } for (uint8_t i=0; iupdate_send(); } WITH_SEMAPHORE(_statustext_sem); service_statustext(); @@ -1883,9 +1861,7 @@ void GCS::update_send() void GCS::update_receive(void) { for (uint8_t i=0; iupdate_receive(); } // also update UART pass-thru, if enabled update_passthru(); @@ -1894,17 +1870,62 @@ void GCS::update_receive(void) void GCS::send_mission_item_reached_message(uint16_t mission_index) { for (uint8_t i=0; imission_item_reached_index = mission_index; + chan(i)->send_message(MSG_MISSION_ITEM_REACHED); } } +void GCS::setup_console() +{ + AP_HAL::UARTDriver *uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_MAVLink, 0); + if (uart == nullptr) { + // this is probably not going to end well. + return; + } + if (ARRAY_SIZE(chan_parameters) == 0) { + return; + } + create_gcs_mavlink_backend(chan_parameters[0], *uart); +} + + +GCS_MAVLINK_Parameters::GCS_MAVLINK_Parameters() +{ + AP_Param::setup_object_defaults(this, var_info); +} + +void GCS::create_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, AP_HAL::UARTDriver &uart) +{ + if (_num_gcs >= ARRAY_SIZE(chan_parameters)) { + return; + } + _chan[_num_gcs] = new_gcs_mavlink_backend(params, uart); + if (_chan[_num_gcs] == nullptr) { + return; + } + + if (!_chan[_num_gcs]->init(_num_gcs)) { + delete _chan[_num_gcs]; + _chan[_num_gcs] = nullptr; + return; + } + + _num_gcs++; +} + void GCS::setup_uarts() { for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) { - chan(i).setup_uart(i); + if (i >= ARRAY_SIZE(chan_parameters)) { + // should not happen + break; + } + AP_HAL::UARTDriver *uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_MAVLink, i); + if (uart == nullptr) { + // no more mavlink uarts + break; + } + create_gcs_mavlink_backend(chan_parameters[i], *uart); } if (frsky == nullptr) { diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index af8a9894a7..c3d5664cbd 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -23,6 +23,12 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] {}; */ class GCS_MAVLINK_Dummy : public GCS_MAVLINK { +public: + + using GCS_MAVLINK::GCS_MAVLINK; + +private: + uint32_t telem_delay() const override { return 0; } void handleMessage(const mavlink_message_t &msg) override {} bool try_send_message(enum ap_message id) override { return true; } @@ -53,10 +59,32 @@ extern const AP_HAL::HAL& hal; class GCS_Dummy : public GCS { - GCS_MAVLINK_Dummy dummy_backend; - uint8_t num_gcs() const override { return 1; } - GCS_MAVLINK_Dummy &chan(const uint8_t ofs) override { return dummy_backend; } - const GCS_MAVLINK_Dummy &chan(const uint8_t ofs) const override { return dummy_backend; }; +public: + + using GCS::GCS; + +protected: + + GCS_MAVLINK_Dummy *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, + AP_HAL::UARTDriver &uart) override { + return new GCS_MAVLINK_Dummy(params, uart); + } + +private: + GCS_MAVLINK_Dummy *chan(const uint8_t ofs) override { + if (ofs > _num_gcs) { + AP::internalerror().error(AP_InternalError::error_t::gcs_offset); + return nullptr; + } + return (GCS_MAVLINK_Dummy *)_chan[ofs]; + }; + const GCS_MAVLINK_Dummy *chan(const uint8_t ofs) const override { + if (ofs > _num_gcs) { + AP::internalerror().error(AP_InternalError::error_t::gcs_offset); + return nullptr; + } + return (GCS_MAVLINK_Dummy *)_chan[ofs]; + }; void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text) override { hal.console->printf("TOGCS: %s\n", text); } diff --git a/libraries/GCS_MAVLink/GCS_Param.cpp b/libraries/GCS_MAVLink/GCS_Param.cpp index 111b46f92c..5cd7824f10 100644 --- a/libraries/GCS_MAVLink/GCS_Param.cpp +++ b/libraries/GCS_MAVLink/GCS_Param.cpp @@ -36,10 +36,6 @@ bool GCS_MAVLINK::param_timer_registered; void GCS_MAVLINK::queued_param_send() { - if (!initialised) { - return; - } - // send parameter async replies uint8_t async_replies_sent_count = send_parameter_async_replies(); diff --git a/libraries/GCS_MAVLink/examples/routing/routing.cpp b/libraries/GCS_MAVLink/examples/routing/routing.cpp index 1fb833a400..51f3b499ca 100644 --- a/libraries/GCS_MAVLink/examples/routing/routing.cpp +++ b/libraries/GCS_MAVLink/examples/routing/routing.cpp @@ -7,17 +7,19 @@ #include #include #include +#include void setup(); void loop(); const AP_HAL::HAL& hal = AP_HAL::get_HAL(); +AP_SerialManager _serialmanager; GCS_Dummy _gcs; extern mavlink_system_t mavlink_system; -const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { +const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { AP_GROUPEND }; @@ -26,7 +28,7 @@ static MAVLink_routing routing; void setup(void) { hal.console->printf("routing test startup..."); - gcs().chan(0).init(hal.uartA, MAVLINK_COMM_0); + gcs().setup_console(); } void loop(void)