diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 65e75a7993..8963047c36 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -360,7 +360,7 @@ void GCS_MAVLINK_Rover::packetReceived(const mavlink_status_t &status, const mav /* 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 @@ -368,7 +368,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 @@ -377,7 +377,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 @@ -386,7 +386,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 @@ -395,7 +395,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 @@ -404,7 +404,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 @@ -413,7 +413,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 @@ -422,7 +422,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 @@ -431,7 +431,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 @@ -440,7 +440,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), // @Param: ADSB // @DisplayName: ADSB stream rate to ground station @@ -449,7 +449,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 }; @@ -1069,9 +1069,6 @@ uint64_t GCS_MAVLINK_Rover::capabilities() const void Rover::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; - if (!gcs().chan(0).initialised) { - return; - } // don't allow potentially expensive logging calls: logger.EnableWrites(false); diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h index affe687e67..1c6d9a8402 100644 --- a/APMrover2/GCS_Mavlink.h +++ b/APMrover2/GCS_Mavlink.h @@ -6,6 +6,8 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK { public: + using GCS_MAVLINK::GCS_MAVLINK; + protected: uint32_t telem_delay() const override; diff --git a/APMrover2/GCS_Rover.h b/APMrover2/GCS_Rover.h index abb0f553ba..8ac6481a46 100644 --- a/APMrover2/GCS_Rover.h +++ b/APMrover2/GCS_Rover.h @@ -9,24 +9,21 @@ class GCS_Rover : 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_Rover &chan(uint8_t ofs) override { - if (ofs >= num_gcs()) { + GCS_MAVLINK_Rover *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_Rover*)_chan[ofs]; } // return GCS link at offset ofs - const GCS_MAVLINK_Rover &chan(uint8_t ofs) const override { - if (ofs >= num_gcs()) { + const GCS_MAVLINK_Rover *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_Rover*)_chan[ofs]; } uint32_t custom_mode() const override; @@ -39,8 +36,11 @@ public: bool simple_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 ¶ms, + AP_HAL::UARTDriver &uart) override { + return new GCS_MAVLINK_Rover(params, uart); + } }; diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 1422adb978..6c20e944d3 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -252,19 +252,19 @@ const AP_Param::Info Rover::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: SERIAL // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index ded181b3fe..8a2f2e3967 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -312,7 +312,7 @@ void ModeAuto::send_guided_position_target() uint8_t compid; mavlink_channel_t 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); } } diff --git a/APMrover2/motor_test.cpp b/APMrover2/motor_test.cpp index 37cd638128..c4b29d185e 100644 --- a/APMrover2/motor_test.cpp +++ b/APMrover2/motor_test.cpp @@ -62,7 +62,11 @@ void Rover::motor_test_output() // 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) { - 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 if (!initialised) { diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index b0e6046141..74ab0aaafb 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -44,7 +44,7 @@ void Rover::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