diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index b4ff02656e..803515e540 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -382,8 +382,6 @@ void Sub::one_hz_loop() // update assigned functions and enable auxiliary servos SRV_Channels::enable_aux_servos(); - check_usb_mux(); - // update position controller alt limits update_poscon_alt_max(); diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index bfe4a486ba..4654aa11f7 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1885,7 +1885,6 @@ void Sub::mavlink_delay_cb() last_5s = tnow; gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM"); } - check_usb_mux(); in_mavlink_delay = false; } diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 619c2b0f44..aa3e2af98e 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -228,7 +228,6 @@ private: struct { uint8_t pre_arm_check : 1; // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed uint8_t logging_started : 1; // true if dataflash logging has started - uint8_t usb_connected : 1; // true if APM is powered from USB connection uint8_t compass_mot : 1; // true if we are currently performing compassmot calibration uint8_t motor_test : 1; // true if we are currently performing the motors test uint8_t initialised : 1; // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes @@ -669,7 +668,6 @@ private: bool position_ok(); bool ekf_position_ok(); bool optflow_position_ok(); - void check_usb_mux(void); bool should_log(uint32_t mask); void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...); bool start_command(const AP_Mission::Mission_Command& cmd); diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 9e7c338708..deb76b2630 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -55,11 +55,6 @@ void Sub::init_ardupilot() // hal.scheduler->delay. hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); - // we start by assuming USB connected, as we initialed the serial - // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. - ap.usb_connected = true; - check_usb_mux(); - // setup telem slots with serial ports for (uint8_t i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) { gcs_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i); @@ -284,17 +279,6 @@ bool Sub::optflow_position_ok() #endif } -void Sub::check_usb_mux(void) -{ - bool usb_check = hal.gpio->usb_connected(); - if (usb_check == ap.usb_connected) { - return; - } - - // the user has switched to/from the telemetry port - ap.usb_connected = usb_check; -} - /* should we log a message type now? */