From ab9614a3de8ec376defe561aa4efb852f2583d3b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 19 Jun 2019 21:13:57 +1000 Subject: [PATCH] Tracker: avoid allocate a GCS_MAVLINK per mavlink channel --- AntennaTracker/GCS_Mavlink.cpp | 23 ++++++++++------------- AntennaTracker/GCS_Mavlink.h | 2 ++ AntennaTracker/GCS_Tracker.cpp | 4 ---- AntennaTracker/GCS_Tracker.h | 28 +++++++++++++++------------- AntennaTracker/Parameters.cpp | 8 ++++---- AntennaTracker/system.cpp | 2 +- 6 files changed, 32 insertions(+), 35 deletions(-) diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 511660aeaa..8d28c68284 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -145,7 +145,7 @@ void GCS_MAVLINK_Tracker::handle_change_alt_request(AP_Mission::Mission_Command& /* default stream rates to 1Hz */ -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: Raw sensor stream rate to ground station @@ -153,7 +153,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], 1), + AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0], 1), // @Param: EXT_STAT // @DisplayName: Extended status stream rate to ground station @@ -162,7 +162,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], 1), + AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1], 1), // @Param: RC_CHAN // @DisplayName: RC Channel stream rate to ground station @@ -171,7 +171,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], 1), + AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 1), // @Param: RAW_CTRL // @DisplayName: Raw Control stream rate to ground station @@ -180,7 +180,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], 1), + AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3], 1), // @Param: POSITION // @DisplayName: Position stream rate to ground station @@ -189,7 +189,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4], 1), + AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4], 1), // @Param: EXTRA1 // @DisplayName: Extra data type 1 stream rate to ground station @@ -198,7 +198,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRates[5], 1), + AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[5], 1), // @Param: EXTRA2 // @DisplayName: Extra data type 2 stream rate to ground station @@ -207,7 +207,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRates[6], 1), + AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[6], 1), // @Param: EXTRA3 // @DisplayName: Extra data type 3 stream rate to ground station @@ -216,7 +216,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRates[7], 1), + AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[7], 1), // @Param: PARAMS // @DisplayName: Parameter stream rate to ground station @@ -225,7 +225,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 10), + AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[8], 10), AP_GROUPEND }; @@ -585,9 +585,6 @@ uint64_t GCS_MAVLINK_Tracker::capabilities() const void Tracker::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; - if (!gcs().chan(0).initialised) { - return; - } logger.EnableWrites(false); diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index 3c0967b88b..b4be60801a 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -7,6 +7,8 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK public: + using GCS_MAVLINK::GCS_MAVLINK; + protected: // telem_delay is not used by Tracker but is pure virtual, thus diff --git a/AntennaTracker/GCS_Tracker.cpp b/AntennaTracker/GCS_Tracker.cpp index ff6be1b32b..378f96949c 100644 --- a/AntennaTracker/GCS_Tracker.cpp +++ b/AntennaTracker/GCS_Tracker.cpp @@ -4,7 +4,6 @@ void GCS_Tracker::request_datastream_position(const uint8_t sysid, const uint8_t compid) { for (uint8_t i=0; i < num_gcs(); i++) { - if (gcs().chan(i).initialised) { // request position if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) { mavlink_msg_request_data_stream_send( @@ -15,14 +14,12 @@ void GCS_Tracker::request_datastream_position(const uint8_t sysid, const uint8_t tracker.g.mavlink_update_rate, 1); // start streaming } - } } } void GCS_Tracker::request_datastream_airpressure(const uint8_t sysid, const uint8_t compid) { for (uint8_t i=0; i < num_gcs(); i++) { - if (gcs().chan(i).initialised) { // request air pressure if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) { mavlink_msg_request_data_stream_send( @@ -33,7 +30,6 @@ void GCS_Tracker::request_datastream_airpressure(const uint8_t sysid, const uint tracker.g.mavlink_update_rate, 1); // start streaming } - } } } diff --git a/AntennaTracker/GCS_Tracker.h b/AntennaTracker/GCS_Tracker.h index 7556c3f39a..64f80159d9 100644 --- a/AntennaTracker/GCS_Tracker.h +++ b/AntennaTracker/GCS_Tracker.h @@ -10,23 +10,20 @@ class GCS_Tracker : 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_Tracker &chan(uint8_t ofs) override { - if (ofs >= num_gcs()) { + GCS_MAVLINK_Tracker *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_Tracker*)_chan[ofs]; } - const GCS_MAVLINK_Tracker &chan(uint8_t ofs) const override { - if (ofs >= num_gcs()) { + const GCS_MAVLINK_Tracker *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_Tracker*)_chan[ofs]; } void update_vehicle_sensor_status_flags() override; @@ -34,11 +31,16 @@ public: uint32_t custom_mode() const override; MAV_TYPE frame_type() const override; +protected: + + GCS_MAVLINK_Tracker *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, + AP_HAL::UARTDriver &uart) override { + return new GCS_MAVLINK_Tracker(params, uart); + } + private: void request_datastream_position(uint8_t sysid, uint8_t compid); void request_datastream_airpressure(uint8_t sysid, uint8_t compid); - GCS_MAVLINK_Tracker _chan[MAVLINK_COMM_NUM_BUFFERS]; - }; diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index a06d384b95..f9127b36cd 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -229,19 +229,19 @@ const AP_Param::Info Tracker::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), // @Param: LOG_BITMASK // @DisplayName: Log bitmask diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 38ece8855b..bd702dbb5d 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -29,7 +29,7 @@ void Tracker::init_tracker() 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