From 48a1b2f43664a9a1371ffaad53b5eb2e60d21099 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 18 Jun 2019 20:38:00 +1000 Subject: [PATCH] Copter: avoid allocate a GCS_MAVLINK per mavlink channel --- ArduCopter/GCS_Copter.h | 26 ++++++++++++-------------- ArduCopter/GCS_Mavlink.cpp | 23 +++++++++++------------ ArduCopter/GCS_Mavlink.h | 2 ++ ArduCopter/Parameters.cpp | 8 ++++---- ArduCopter/system.cpp | 3 +-- 5 files changed, 30 insertions(+), 32 deletions(-) diff --git a/ArduCopter/GCS_Copter.h b/ArduCopter/GCS_Copter.h index 5a899aa9fd..d394df616e 100644 --- a/ArduCopter/GCS_Copter.h +++ b/ArduCopter/GCS_Copter.h @@ -9,23 +9,20 @@ class GCS_Copter : 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_Copter &chan(uint8_t ofs) override { - if (ofs >= num_gcs()) { + GCS_MAVLINK_Copter *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]; + return (GCS_MAVLINK_Copter *)_chan[ofs]; } - const GCS_MAVLINK_Copter &chan(uint8_t ofs) const override { - if (ofs >= num_gcs()) { + const GCS_MAVLINK_Copter *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_Copter *)_chan[ofs]; } void update_vehicle_sensor_status_flags(void) override; @@ -50,8 +47,9 @@ protected: return 250; } -private: - - GCS_MAVLINK_Copter _chan[MAVLINK_COMM_NUM_BUFFERS]; + GCS_MAVLINK_Copter *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, + AP_HAL::UARTDriver &uart) override { + return new GCS_MAVLINK_Copter(params, uart); + } }; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 6e0e50e628..7ac23b070b 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -284,7 +284,7 @@ bool GCS_MAVLINK_Copter::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_IMU3, SCALED_PRESSURE, SCALED_PRESSURE2, SCALED_PRESSURE3 and SENSOR_OFFSETS to ground station @@ -292,7 +292,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRates[0], 0), + AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0], 0), // @Param: EXT_STAT // @DisplayName: Extended status stream rate to ground station @@ -301,7 +301,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRates[1], 0), + AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1], 0), // @Param: RC_CHAN // @DisplayName: RC Channel stream rate to ground station @@ -310,7 +310,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRates[2], 0), + AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 0), // @Param: RAW_CTRL // @DisplayName: Raw Control stream rate to ground station @@ -319,7 +319,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRates[3], 0), + AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3], 0), // @Param: POSITION // @DisplayName: Position stream rate to ground station @@ -328,7 +328,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4], 0), + AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4], 0), // @Param: EXTRA1 // @DisplayName: Extra data type 1 stream rate to ground station @@ -337,7 +337,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRates[5], 0), + AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[5], 0), // @Param: EXTRA2 // @DisplayName: Extra data type 2 stream rate to ground station @@ -346,7 +346,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRates[6], 0), + AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[6], 0), // @Param: EXTRA3 // @DisplayName: Extra data type 3 stream rate to ground station @@ -355,7 +355,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRates[7], 0), + AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[7], 0), // @Param: PARAMS // @DisplayName: Parameter stream rate to ground station @@ -364,7 +364,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 0), + AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[8], 0), // @Param: ADSB // @DisplayName: ADSB stream rate to ground station @@ -373,7 +373,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 50 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("ADSB", 9, GCS_MAVLINK, streamRates[9], 0), + AP_GROUPINFO("ADSB", 9, GCS_MAVLINK_Parameters, streamRates[9], 0), AP_GROUPEND }; @@ -1240,7 +1240,6 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) void Copter::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; - if (!gcs().chan(0).initialised) return; logger.EnableWrites(false); diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 8477549ed2..642eea6b17 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -7,6 +7,8 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK public: + using GCS_MAVLINK::GCS_MAVLINK; + protected: uint32_t telem_delay() const override; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 489bb62b6a..bdb0551dae 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -530,19 +530,19 @@ const AP_Param::Info Copter::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/ArduCopter/system.cpp b/ArduCopter/system.cpp index 9d8e7dd27c..b430bbedcd 100755 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -52,8 +52,7 @@ void Copter::init_ardupilot() serial_manager.init(); // setup first port early to allow BoardConfig to report errors - gcs().chan(0).setup_uart(0); - + gcs().setup_console(); // Register mavlink_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay