diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 30ed2c7812..f67f842b57 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -450,7 +450,7 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) /* 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 @@ -458,7 +458,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 @@ -467,7 +467,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 @@ -476,7 +476,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 @@ -485,7 +485,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 @@ -494,7 +494,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 @@ -503,7 +503,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 @@ -512,7 +512,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 @@ -521,7 +521,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 @@ -530,7 +530,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 @@ -539,7 +539,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Range: 0 50 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("ADSB", 9, GCS_MAVLINK, streamRates[9], 5), + AP_GROUPINFO("ADSB", 9, GCS_MAVLINK_Parameters, streamRates[9], 5), AP_GROUPEND }; @@ -1349,7 +1349,6 @@ void GCS_MAVLINK_Plane::handle_rc_channels_override(const mavlink_message_t &msg void Plane::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; - if (!gcs().chan(0).initialised) return; logger.EnableWrites(false); diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 5f799a2003..7ef5f22748 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -8,6 +8,8 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK public: + using GCS_MAVLINK::GCS_MAVLINK; + protected: uint32_t telem_delay() const override; diff --git a/ArduPlane/GCS_Plane.h b/ArduPlane/GCS_Plane.h index e6cef69f8b..3e47988d94 100644 --- a/ArduPlane/GCS_Plane.h +++ b/ArduPlane/GCS_Plane.h @@ -9,23 +9,20 @@ class GCS_Plane : 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_Plane &chan(uint8_t ofs) override { - if (ofs >= num_gcs()) { + GCS_MAVLINK_Plane *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_Plane *)_chan[ofs]; } - const GCS_MAVLINK_Plane &chan(uint8_t ofs) const override { - if (ofs >= num_gcs()) { + const GCS_MAVLINK_Plane *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_Plane *)_chan[ofs]; } protected: @@ -34,8 +31,8 @@ protected: uint32_t custom_mode() const override; MAV_TYPE frame_type() const override; -private: - - GCS_MAVLINK_Plane _chan[MAVLINK_COMM_NUM_BUFFERS]; - + GCS_MAVLINK_Plane *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, + AP_HAL::UARTDriver &uart) override { + return new GCS_MAVLINK_Plane(params, uart); + } }; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index fa60e3fa90..666e5da249 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -970,19 +970,19 @@ const AP_Param::Info Plane::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: INS_ // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 1c909ec089..7f2e243aaf 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -55,8 +55,7 @@ void Plane::init_ardupilot() // initialise serial ports serial_manager.init(); - 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 @@ -343,8 +342,8 @@ void Plane::check_long_failsafe() (tnow - failsafe.last_heartbeat_ms) > g.fs_timeout_long*1000) { failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI && - gcs().chan(0).last_radio_status_remrssi_ms != 0 && - (tnow - gcs().chan(0).last_radio_status_remrssi_ms) > g.fs_timeout_long*1000) { + gcs().chan(0)->last_radio_status_remrssi_ms != 0 && + (tnow - gcs().chan(0)->last_radio_status_remrssi_ms) > g.fs_timeout_long*1000) { failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); } } else {