zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Tracker: avoid allocate a GCS_MAVLINK per mavlink channel

master
Peter Barker 6 years ago committed by Peter Barker
parent
45af50f8bd
commit
ab9614a3de
6 changed files with 32 additions and 35 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 23
      AntennaTracker/GCS_Mavlink.cpp
  2. 2
      AntennaTracker/GCS_Mavlink.h
  3. 4
      AntennaTracker/GCS_Tracker.cpp
  4. 28
      AntennaTracker/GCS_Tracker.h
  5. 8
      AntennaTracker/Parameters.cpp
  6. 2
      AntennaTracker/system.cpp

23
AntennaTracker/GCS_Mavlink.cpp
Unescape View File

@ -145,7 +145,7 @@ void GCS_MAVLINK_Tracker::handle_change_alt_request(AP_Mission::Mission_Command&
/* /*
default stream rates to 1Hz 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 // @Param: RAW_SENS
// @DisplayName: Raw sensor stream rate // @DisplayName: Raw sensor stream rate
// @Description: Raw sensor stream rate to ground station // @Description: Raw sensor stream rate to ground station
@ -153,7 +153,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
// @Range: 0 10 // @Range: 0 10
// @Increment: 1 // @Increment: 1
// @User: Advanced // @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 // @Param: EXT_STAT
// @DisplayName: Extended status stream rate to ground station // @DisplayName: Extended status stream rate to ground station
@ -162,7 +162,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
// @Range: 0 10 // @Range: 0 10
// @Increment: 1 // @Increment: 1
// @User: Advanced // @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 // @Param: RC_CHAN
// @DisplayName: RC Channel stream rate to ground station // @DisplayName: RC Channel stream rate to ground station
@ -171,7 +171,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
// @Range: 0 10 // @Range: 0 10
// @Increment: 1 // @Increment: 1
// @User: Advanced // @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 // @Param: RAW_CTRL
// @DisplayName: Raw Control stream rate to ground station // @DisplayName: Raw Control stream rate to ground station
@ -180,7 +180,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
// @Range: 0 10 // @Range: 0 10
// @Increment: 1 // @Increment: 1
// @User: Advanced // @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 // @Param: POSITION
// @DisplayName: Position stream rate to ground station // @DisplayName: Position stream rate to ground station
@ -189,7 +189,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
// @Range: 0 10 // @Range: 0 10
// @Increment: 1 // @Increment: 1
// @User: Advanced // @User: Advanced
AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4], 1), AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4], 1),
// @Param: EXTRA1 // @Param: EXTRA1
// @DisplayName: Extra data type 1 stream rate to ground station // @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 // @Range: 0 10
// @Increment: 1 // @Increment: 1
// @User: Advanced // @User: Advanced
AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRates[5], 1), AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[5], 1),
// @Param: EXTRA2 // @Param: EXTRA2
// @DisplayName: Extra data type 2 stream rate to ground station // @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 // @Range: 0 10
// @Increment: 1 // @Increment: 1
// @User: Advanced // @User: Advanced
AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRates[6], 1), AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[6], 1),
// @Param: EXTRA3 // @Param: EXTRA3
// @DisplayName: Extra data type 3 stream rate to ground station // @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 // @Range: 0 10
// @Increment: 1 // @Increment: 1
// @User: Advanced // @User: Advanced
AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRates[7], 1), AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[7], 1),
// @Param: PARAMS // @Param: PARAMS
// @DisplayName: Parameter stream rate to ground station // @DisplayName: Parameter stream rate to ground station
@ -225,7 +225,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
// @Range: 0 10 // @Range: 0 10
// @Increment: 1 // @Increment: 1
// @User: Advanced // @User: Advanced
AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 10), AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[8], 10),
AP_GROUPEND AP_GROUPEND
}; };
@ -585,9 +585,6 @@ uint64_t GCS_MAVLINK_Tracker::capabilities() const
void Tracker::mavlink_delay_cb() void Tracker::mavlink_delay_cb()
{ {
static uint32_t last_1hz, last_50hz, last_5s; static uint32_t last_1hz, last_50hz, last_5s;
if (!gcs().chan(0).initialised) {
return;
}
logger.EnableWrites(false); logger.EnableWrites(false);

2
AntennaTracker/GCS_Mavlink.h
Unescape View File

@ -7,6 +7,8 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK
public: public:
using GCS_MAVLINK::GCS_MAVLINK;
protected: protected:
// telem_delay is not used by Tracker but is pure virtual, thus // telem_delay is not used by Tracker but is pure virtual, thus

4
AntennaTracker/GCS_Tracker.cpp
Unescape View File

@ -4,7 +4,6 @@
void GCS_Tracker::request_datastream_position(const uint8_t sysid, const uint8_t compid) void GCS_Tracker::request_datastream_position(const uint8_t sysid, const uint8_t compid)
{ {
for (uint8_t i=0; i < num_gcs(); i++) { for (uint8_t i=0; i < num_gcs(); i++) {
if (gcs().chan(i).initialised) {
// request position // request position
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) { if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
mavlink_msg_request_data_stream_send( mavlink_msg_request_data_stream_send(
@ -16,13 +15,11 @@ void GCS_Tracker::request_datastream_position(const uint8_t sysid, const uint8_t
1); // start streaming 1); // start streaming
} }
} }
}
} }
void GCS_Tracker::request_datastream_airpressure(const uint8_t sysid, const uint8_t compid) void GCS_Tracker::request_datastream_airpressure(const uint8_t sysid, const uint8_t compid)
{ {
for (uint8_t i=0; i < num_gcs(); i++) { for (uint8_t i=0; i < num_gcs(); i++) {
if (gcs().chan(i).initialised) {
// request air pressure // request air pressure
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) { if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
mavlink_msg_request_data_stream_send( mavlink_msg_request_data_stream_send(
@ -34,7 +31,6 @@ void GCS_Tracker::request_datastream_airpressure(const uint8_t sysid, const uint
1); // start streaming 1); // start streaming
} }
} }
}
} }
// update sensors and subsystems present, enabled and healthy flags for reporting to GCS // update sensors and subsystems present, enabled and healthy flags for reporting to GCS

28
AntennaTracker/GCS_Tracker.h
Unescape View File

@ -10,23 +10,20 @@ class GCS_Tracker : public GCS
public: public:
// return the number of valid GCS objects
uint8_t num_gcs() const override { return ARRAY_SIZE(_chan); };
// return GCS link at offset ofs // return GCS link at offset ofs
GCS_MAVLINK_Tracker &chan(uint8_t ofs) override { GCS_MAVLINK_Tracker *chan(const uint8_t ofs) override {
if (ofs >= num_gcs()) { if (ofs > _num_gcs) {
AP::internalerror().error(AP_InternalError::error_t::gcs_offset); 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 { const GCS_MAVLINK_Tracker *chan(const uint8_t ofs) const override {
if (ofs >= num_gcs()) { if (ofs > _num_gcs) {
AP::internalerror().error(AP_InternalError::error_t::gcs_offset); 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; void update_vehicle_sensor_status_flags() override;
@ -34,11 +31,16 @@ public:
uint32_t custom_mode() const override; uint32_t custom_mode() const override;
MAV_TYPE frame_type() const override; MAV_TYPE frame_type() const override;
protected:
GCS_MAVLINK_Tracker *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
AP_HAL::UARTDriver &uart) override {
return new GCS_MAVLINK_Tracker(params, uart);
}
private: private:
void request_datastream_position(uint8_t sysid, uint8_t compid); void request_datastream_position(uint8_t sysid, uint8_t compid);
void request_datastream_airpressure(uint8_t sysid, uint8_t compid); void request_datastream_airpressure(uint8_t sysid, uint8_t compid);
GCS_MAVLINK_Tracker _chan[MAVLINK_COMM_NUM_BUFFERS];
}; };

8
AntennaTracker/Parameters.cpp
Unescape View File

@ -229,19 +229,19 @@ const AP_Param::Info Tracker::var_info[] = {
// @Group: SR0_ // @Group: SR0_
// @Path: GCS_Mavlink.cpp // @Path: GCS_Mavlink.cpp
GOBJECTN(gcs().chan(0), gcs0, "SR0_", GCS_MAVLINK), GOBJECTN(_gcs.chan_parameters[0], gcs0, "SR0_", GCS_MAVLINK_Parameters),
// @Group: SR1_ // @Group: SR1_
// @Path: GCS_Mavlink.cpp // @Path: GCS_Mavlink.cpp
GOBJECTN(gcs().chan(1), gcs1, "SR1_", GCS_MAVLINK), GOBJECTN(_gcs.chan_parameters[1], gcs1, "SR1_", GCS_MAVLINK_Parameters),
// @Group: SR2_ // @Group: SR2_
// @Path: GCS_Mavlink.cpp // @Path: GCS_Mavlink.cpp
GOBJECTN(gcs().chan(2), gcs2, "SR2_", GCS_MAVLINK), GOBJECTN(_gcs.chan_parameters[2], gcs2, "SR2_", GCS_MAVLINK_Parameters),
// @Group: SR3_ // @Group: SR3_
// @Path: GCS_Mavlink.cpp // @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 // @Param: LOG_BITMASK
// @DisplayName: Log bitmask // @DisplayName: Log bitmask

2
AntennaTracker/system.cpp
Unescape View File

@ -29,7 +29,7 @@ void Tracker::init_tracker()
serial_manager.init(); serial_manager.init();
// setup first port early to allow BoardConfig to report errors // 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 // Register mavlink_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay // more than 5ms remaining in your call to hal.scheduler->delay

Write Preview
Loading…
Cancel
Save