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

Rover: avoid allocate a GCS_MAVLINK per mavlink channel

master
Peter Barker 6 years ago committed by Peter Barker
parent
ab9614a3de
commit
190f716c43
7 changed files with 37 additions and 34 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. 25
      APMrover2/GCS_Mavlink.cpp
  2. 2
      APMrover2/GCS_Mavlink.h
  3. 26
      APMrover2/GCS_Rover.h
  4. 8
      APMrover2/Parameters.cpp
  5. 2
      APMrover2/mode_auto.cpp
  6. 6
      APMrover2/motor_test.cpp
  7. 2
      APMrover2/system.cpp

25
APMrover2/GCS_Mavlink.cpp
Unescape View File

@ -360,7 +360,7 @@ void GCS_MAVLINK_Rover::packetReceived(const mavlink_status_t &status, const mav
/* /*
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
@ -368,7 +368,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
@ -377,7 +377,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
@ -386,7 +386,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
@ -395,7 +395,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
@ -404,7 +404,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
@ -413,7 +413,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
@ -422,7 +422,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
@ -431,7 +431,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
@ -440,7 +440,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),
// @Param: ADSB // @Param: ADSB
// @DisplayName: ADSB stream rate to ground station // @DisplayName: ADSB stream rate to ground station
@ -449,7 +449,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
// @Range: 0 50 // @Range: 0 50
// @Increment: 1 // @Increment: 1
// @User: Advanced // @User: Advanced
AP_GROUPINFO("ADSB", 9, GCS_MAVLINK, streamRates[9], 0), AP_GROUPINFO("ADSB", 9, GCS_MAVLINK_Parameters, streamRates[9], 0),
AP_GROUPEND AP_GROUPEND
}; };
@ -1069,9 +1069,6 @@ uint64_t GCS_MAVLINK_Rover::capabilities() const
void Rover::mavlink_delay_cb() void Rover::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;
}
// don't allow potentially expensive logging calls: // don't allow potentially expensive logging calls:
logger.EnableWrites(false); logger.EnableWrites(false);

2
APMrover2/GCS_Mavlink.h
Unescape View File

@ -6,6 +6,8 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK
{ {
public: public:
using GCS_MAVLINK::GCS_MAVLINK;
protected: protected:
uint32_t telem_delay() const override; uint32_t telem_delay() const override;

26
APMrover2/GCS_Rover.h
Unescape View File

@ -9,24 +9,21 @@ class GCS_Rover : 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_Rover &chan(uint8_t ofs) override { GCS_MAVLINK_Rover *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_Rover*)_chan[ofs];
} }
// return GCS link at offset ofs // return GCS link at offset ofs
const GCS_MAVLINK_Rover &chan(uint8_t ofs) const override { const GCS_MAVLINK_Rover *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_Rover*)_chan[ofs];
} }
uint32_t custom_mode() const override; uint32_t custom_mode() const override;
@ -39,8 +36,11 @@ public:
bool simple_input_active() const override; bool simple_input_active() const override;
bool supersimple_input_active() const override; bool supersimple_input_active() const override;
private: protected:
GCS_MAVLINK_Rover _chan[MAVLINK_COMM_NUM_BUFFERS]; GCS_MAVLINK_Rover *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
AP_HAL::UARTDriver &uart) override {
return new GCS_MAVLINK_Rover(params, uart);
}
}; };

8
APMrover2/Parameters.cpp
Unescape View File

@ -252,19 +252,19 @@ const AP_Param::Info Rover::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),
// @Group: SERIAL // @Group: SERIAL
// @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp

2
APMrover2/mode_auto.cpp
Unescape View File

@ -312,7 +312,7 @@ void ModeAuto::send_guided_position_target()
uint8_t compid; uint8_t compid;
mavlink_channel_t chan; mavlink_channel_t chan;
if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_ONBOARD_CONTROLLER, sysid, compid, chan)) { if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_ONBOARD_CONTROLLER, sysid, compid, chan)) {
gcs().chan(chan-MAVLINK_COMM_0).send_set_position_target_global_int(sysid, compid, guided_target.loc); gcs().chan(chan-MAVLINK_COMM_0)->send_set_position_target_global_int(sysid, compid, guided_target.loc);
} }
} }

6
APMrover2/motor_test.cpp
Unescape View File

@ -62,7 +62,11 @@ void Rover::motor_test_output()
// return true if tests can continue, false if not // return true if tests can continue, false if not
bool Rover::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc, uint8_t motor_seq, uint8_t throttle_type, int16_t throttle_value) bool Rover::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc, uint8_t motor_seq, uint8_t throttle_type, int16_t throttle_value)
{ {
GCS_MAVLINK_Rover &gcs_chan = gcs().chan(chan-MAVLINK_COMM_0); GCS_MAVLINK_Rover *gcs_chan_ptr = gcs().chan(chan-MAVLINK_COMM_0);
if (gcs_chan_ptr == nullptr) {
return false;
}
GCS_MAVLINK_Rover &gcs_chan = *gcs_chan_ptr;
// check board has initialised // check board has initialised
if (!initialised) { if (!initialised) {

2
APMrover2/system.cpp
Unescape View File

@ -44,7 +44,7 @@ void Rover::init_ardupilot()
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