From 45af50f8bd5e06ac65548fa6549615c46351e9b3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 19 Jun 2019 20:32:27 +1000 Subject: [PATCH] Sub: avoid allocate a GCS_MAVLINK per mavlink channel --- ArduSub/GCS_Mavlink.cpp | 21 +++++++++------------ ArduSub/GCS_Mavlink.h | 2 ++ ArduSub/GCS_Sub.h | 30 ++++++++++++++---------------- ArduSub/Parameters.cpp | 8 ++++---- ArduSub/system.cpp | 2 +- 5 files changed, 30 insertions(+), 33 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 62584e73d5..4f1d8bae86 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -240,7 +240,7 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id) } -const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { +const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Param: RAW_SENS // @DisplayName: Raw sensor stream rate // @Description: Stream rate of RAW_IMU, SCALED_IMU2, SCALED_PRESSURE, and SENSOR_OFFSETS to ground station @@ -248,7 +248,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRates[STREAM_RAW_SENSORS], 0), + AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_RAW_SENSORS], 0), // @Param: EXT_STAT // @DisplayName: Extended status stream rate to ground station @@ -257,7 +257,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRates[STREAM_EXTENDED_STATUS], 0), + AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTENDED_STATUS], 0), // @Param: RC_CHAN // @DisplayName: RC Channel stream rate to ground station @@ -266,7 +266,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRates[STREAM_RC_CHANNELS], 0), + AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_RC_CHANNELS], 0), // @Param: POSITION // @DisplayName: Position stream rate to ground station @@ -275,7 +275,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[STREAM_POSITION], 0), + AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_POSITION], 0), // @Param: EXTRA1 // @DisplayName: Extra data type 1 stream rate to ground station @@ -284,7 +284,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRates[STREAM_EXTRA1], 0), + AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA1], 0), // @Param: EXTRA2 // @DisplayName: Extra data type 2 stream rate to ground station @@ -293,7 +293,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRates[STREAM_EXTRA2], 0), + AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA2], 0), // @Param: EXTRA3 // @DisplayName: Extra data type 3 stream rate to ground station @@ -302,7 +302,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRates[STREAM_EXTRA3], 0), + AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA3], 0), // @Param: PARAMS // @DisplayName: Parameter stream rate to ground station @@ -311,7 +311,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[STREAM_PARAMS], 0), + AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_PARAMS], 0), AP_GROUPEND }; @@ -794,9 +794,6 @@ void GCS_MAVLINK_Sub::handle_rc_channels_override(const mavlink_message_t &msg) void Sub::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; - if (!gcs().chan(0).initialised) { - return; - } logger.EnableWrites(false); diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index 697567cfbf..c6e416c575 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -6,6 +6,8 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { public: + using GCS_MAVLINK::GCS_MAVLINK; + protected: uint32_t telem_delay() const override { diff --git a/ArduSub/GCS_Sub.h b/ArduSub/GCS_Sub.h index 8a819af419..b2c7bfc5ea 100644 --- a/ArduSub/GCS_Sub.h +++ b/ArduSub/GCS_Sub.h @@ -9,24 +9,21 @@ class GCS_Sub : public GCS 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_Sub &chan(uint8_t ofs) override { - if (ofs >= num_gcs()) { + GCS_MAVLINK_Sub *chan(const uint8_t ofs) override { + if (ofs > _num_gcs) { AP::internalerror().error(AP_InternalError::error_t::gcs_offset); - ofs = 0; + return nullptr; } - return _chan[ofs]; - }; - const GCS_MAVLINK_Sub &chan(uint8_t ofs) const override { - if (ofs >= num_gcs()) { + return (GCS_MAVLINK_Sub*)_chan[ofs]; + } + const GCS_MAVLINK_Sub *chan(const uint8_t ofs) const override { + if (ofs > _num_gcs) { AP::internalerror().error(AP_InternalError::error_t::gcs_offset); - ofs = 0; + return nullptr; } - return _chan[ofs]; - }; + return (GCS_MAVLINK_Sub*)_chan[ofs]; + } void update_vehicle_sensor_status_flags() override; @@ -45,8 +42,9 @@ protected: return 250; } -private: - - GCS_MAVLINK_Sub _chan[MAVLINK_COMM_NUM_BUFFERS]; + GCS_MAVLINK_Sub *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, + AP_HAL::UARTDriver &uart) override { + return new GCS_MAVLINK_Sub(params, uart); + } }; diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index a0d390e08b..b2fc8248cb 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -450,19 +450,19 @@ const AP_Param::Info Sub::var_info[] = { // @Group: SR0_ // @Path: GCS_Mavlink.cpp - GOBJECTN(_gcs._chan[0], gcs0, "SR0_", GCS_MAVLINK), + GOBJECTN(_gcs.chan_parameters[0], gcs0, "SR0_", GCS_MAVLINK_Parameters), // @Group: SR1_ // @Path: GCS_Mavlink.cpp - GOBJECTN(_gcs._chan[1], gcs1, "SR1_", GCS_MAVLINK), + GOBJECTN(_gcs.chan_parameters[1], gcs1, "SR1_", GCS_MAVLINK_Parameters), // @Group: SR2_ // @Path: GCS_Mavlink.cpp - GOBJECTN(_gcs._chan[2], gcs2, "SR2_", GCS_MAVLINK), + GOBJECTN(_gcs.chan_parameters[2], gcs2, "SR2_", GCS_MAVLINK_Parameters), // @Group: SR3_ // @Path: GCS_Mavlink.cpp - GOBJECTN(_gcs._chan[3], gcs3, "SR3_", GCS_MAVLINK), + GOBJECTN(_gcs.chan_parameters[3], gcs3, "SR3_", GCS_MAVLINK_Parameters), // @Group: AHRS_ // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index a963dfe2c5..4a2d72416e 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -56,7 +56,7 @@ void Sub::init_ardupilot() serial_manager.init(); // setup first port early to allow BoardConfig to report errors - gcs().chan(0).setup_uart(0); + gcs().setup_console(); // init cargo gripper #if GRIPPER_ENABLED == ENABLED