diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 348cfcd954..df1130d955 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -82,7 +82,7 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan) uint8_t mav_type; mav_type = MAV_TYPE_SUBMARINE; - gcs[chan-MAVLINK_COMM_0].send_heartbeat(mav_type, + gcs_chan[chan-MAVLINK_COMM_0].send_heartbeat(mav_type, base_mode, custom_mode, system_status); @@ -1969,7 +1969,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) void Sub::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; - if (!gcs[0].initialised || in_mavlink_delay) { + if (!gcs_chan[0].initialised || in_mavlink_delay) { return; } @@ -2003,8 +2003,8 @@ void Sub::mavlink_delay_cb() void Sub::gcs_send_message(enum ap_message id) { for (uint8_t i=0; i<num_gcs; i++) { - if (gcs[i].initialised) { - gcs[i].send_message(id); + if (gcs_chan[i].initialised) { + gcs_chan[i].send_message(id); } } } @@ -2015,9 +2015,9 @@ void Sub::gcs_send_message(enum ap_message id) void Sub::gcs_send_mission_item_reached_message(uint16_t mission_index) { for (uint8_t i=0; i<num_gcs; i++) { - if (gcs[i].initialised) { - gcs[i].mission_item_reached_index = mission_index; - gcs[i].send_message(MSG_MISSION_ITEM_REACHED); + if (gcs_chan[i].initialised) { + gcs_chan[i].mission_item_reached_index = mission_index; + gcs_chan[i].send_message(MSG_MISSION_ITEM_REACHED); } } } @@ -2028,8 +2028,8 @@ void Sub::gcs_send_mission_item_reached_message(uint16_t mission_index) void Sub::gcs_data_stream_send(void) { for (uint8_t i=0; i<num_gcs; i++) { - if (gcs[i].initialised) { - gcs[i].data_stream_send(); + if (gcs_chan[i].initialised) { + gcs_chan[i].data_stream_send(); } } } @@ -2040,11 +2040,11 @@ void Sub::gcs_data_stream_send(void) void Sub::gcs_check_input(void) { for (uint8_t i=0; i<num_gcs; i++) { - if (gcs[i].initialised) { + if (gcs_chan[i].initialised) { #if CLI_ENABLED == ENABLED - gcs[i].update(g.cli_enabled==1?FUNCTOR_BIND_MEMBER(&Sub::run_cli, void, AP_HAL::UARTDriver *):NULL); + gcs_chan[i].update(g.cli_enabled==1?FUNCTOR_BIND_MEMBER(&Sub::run_cli, void, AP_HAL::UARTDriver *):NULL); #else - gcs[i].update(NULL); + gcs_chan[i].update(NULL); #endif } } diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index a914cca865..b5378522c1 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -744,7 +744,7 @@ void Sub::log_init(void) DataFlash.Prep(); gcs_send_text(MAV_SEVERITY_INFO, "Prepared log system"); for (uint8_t i=0; i<num_gcs; i++) { - gcs[i].reset_cli_timeout(); + gcs_chan[i].reset_cli_timeout(); } } } diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 862eb0b665..70adb75330 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -720,19 +720,19 @@ const AP_Param::Info Sub::var_info[] = { // @Group: SR0_ // @Path: GCS_Mavlink.cpp - GOBJECTN(gcs[0], gcs0, "SR0_", GCS_MAVLINK), + GOBJECTN(gcs_chan[0], gcs0, "SR0_", GCS_MAVLINK), // @Group: SR1_ // @Path: GCS_Mavlink.cpp - GOBJECTN(gcs[1], gcs1, "SR1_", GCS_MAVLINK), + GOBJECTN(gcs_chan[1], gcs1, "SR1_", GCS_MAVLINK), // @Group: SR2_ // @Path: GCS_Mavlink.cpp - GOBJECTN(gcs[2], gcs2, "SR2_", GCS_MAVLINK), + GOBJECTN(gcs_chan[2], gcs2, "SR2_", GCS_MAVLINK), // @Group: SR3_ // @Path: GCS_Mavlink.cpp - GOBJECTN(gcs[3], gcs3, "SR3_", GCS_MAVLINK), + GOBJECTN(gcs_chan[3], gcs3, "SR3_", GCS_MAVLINK), // @Group: AHRS_ // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 11d8eb4089..63e9fad622 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -219,7 +219,7 @@ private: AP_SerialManager serial_manager; static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; - GCS_MAVLINK_Sub gcs[MAVLINK_COMM_NUM_BUFFERS]; + GCS_MAVLINK_Sub gcs_chan[MAVLINK_COMM_NUM_BUFFERS]; // User variables #ifdef USERHOOK_VARIABLES diff --git a/ArduSub/compassmot.cpp b/ArduSub/compassmot.cpp index d34f595db4..a7d2203851 100644 --- a/ArduSub/compassmot.cpp +++ b/ArduSub/compassmot.cpp @@ -36,7 +36,7 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) // check compass is enabled if (!g.compass_enabled) { - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Compass disabled"); + gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Compass disabled"); ap.compass_mot = false; return 1; } @@ -45,7 +45,7 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) compass.read(); for (uint8_t i=0; i<compass.get_count(); i++) { if (!compass.healthy(i)) { - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Check compass"); + gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Check compass"); ap.compass_mot = false; return 1; } @@ -54,7 +54,7 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) // check if radio is calibrated pre_arm_rc_checks(); if (!ap.pre_arm_rc_check) { - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "RC not calibrated"); + gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "RC not calibrated"); ap.compass_mot = false; return 1; } @@ -62,7 +62,7 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) // check throttle is at zero read_radio(); if (channel_throttle->get_control_in() != 0) { - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Throttle not zero"); + gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Throttle not zero"); ap.compass_mot = false; return 1; } @@ -87,13 +87,13 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) AP_Notify::flags.esc_calibration = true; // warn user we are starting calibration - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Starting calibration"); + gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Starting calibration"); // inform what type of compensation we are attempting if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) { - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Current"); + gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Current"); } else { - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Throttle"); + gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Throttle"); } // disable battery failsafe @@ -236,10 +236,10 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) } compass.save_motor_compensation(); // display success message - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Calibration successful"); + gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Calibration successful"); } else { // compensation vector never updated, report failure - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_NOTICE, "Failed"); + gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_NOTICE, "Failed"); compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); } diff --git a/ArduSub/motor_test.cpp b/ArduSub/motor_test.cpp index de0f4270d3..a34fe54fa3 100644 --- a/ArduSub/motor_test.cpp +++ b/ArduSub/motor_test.cpp @@ -69,13 +69,13 @@ bool Sub::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc) // check rc has been calibrated pre_arm_rc_checks(); if (check_rc && !ap.pre_arm_rc_check) { - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated"); + gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated"); return false; } // check if safety switch has been pushed if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Safety switch"); + gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Safety switch"); return false; } diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 3b08442a4e..07b4665f29 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -137,7 +137,7 @@ void Sub::init_ardupilot() // setup telem slots with serial ports for (uint8_t i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) { - gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i); + gcs_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i); } // identify ourselves correctly with the ground station @@ -208,11 +208,11 @@ void Sub::init_ardupilot() if (g.cli_enabled) { const char *msg = "\nPress ENTER 3 times to start interactive setup\n"; cliSerial->println(msg); - if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) { - gcs[1].get_uart()->println(msg); + if (gcs_chan[1].initialised && (gcs_chan[1].get_uart() != NULL)) { + gcs_chan[1].get_uart()->println(msg); } - if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) { - gcs[2].get_uart()->println(msg); + if (num_gcs > 2 && gcs_chan[2].initialised && (gcs_chan[2].get_uart() != NULL)) { + gcs_chan[2].get_uart()->println(msg); } } #endif // CLI_ENABLED