Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tracker: avoid allocate a GCS_MAVLINK per mavlink channel

This commit is contained in:
Peter Barker 2019-06-19 21:13:57 +10:00 committed by Peter Barker
parent 45af50f8bd
commit ab9614a3de
6 changed files with 32 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
AntennaTracker/GCS_Mavlink.cpp
View File

@ -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);

2
AntennaTracker/GCS_Mavlink.h
View File

@ -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

4
AntennaTracker/GCS_Tracker.cpp
View File

@ -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(
@ -16,13 +15,11 @@ void GCS_Tracker::request_datastream_position(const uint8_t sysid, const uint8_t
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(
@ -34,7 +31,6 @@ void GCS_Tracker::request_datastream_airpressure(const uint8_t sysid, const uint
1); // start streaming
}
}
}
}
// update sensors and subsystems present, enabled and healthy flags for reporting to GCS

28
AntennaTracker/GCS_Tracker.h
View File

@ -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 &params,
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];
};

8
AntennaTracker/Parameters.cpp
View File

@ -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

2
AntennaTracker/system.cpp
View File

@ -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