diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 2b0b7ea6f6..ecef18f6c2 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -445,20 +445,13 @@ void AP_Mount::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_m } } -// update - give mount opportunity to update servos. should be called at 10hz or higher // update - give mount opportunity to update servos. should be called at 10hz or higher void AP_Mount::update(uint8_t mount_compid, AP_SerialManager& serial_manager) { #if AP_AHRS_NAVEKF_AVAILABLE static AP_HAL::UARTDriver *uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_MAVLink,1); - //static bool begin_gmb_uart; - //static uint32_t baud = serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_MAVLink,1); + for (uint8_t instance=0; instance MAX_RETRIES) { //no mavlink gimbal found - break; - } - */ MountType mount_type = get_mount_type(instance); // check for MAVLink mounts if (mount_type == Mount_Type_MAVLink && !_mav_gimbal_found) { @@ -481,40 +474,6 @@ void AP_Mount::update(uint8_t mount_compid, AP_SerialManager& serial_manager) } } } - /* - } else if(_timeout) { - //change baud rate of gimbal port and retry - if(baud == 921600) { - hal.console->printf("Looking for R10C Gimbal!!\n"); - uart->end(); - baud = 230400; - } else if(baud == 230400) { - hal.console->printf("Looking for Solo Gimbal!!\n"); - uart->end(); - baud = 921600; - } else { - hal.console->printf("Setting Back to Default Baud: %d!!\n", 921600); - uart->end(); - baud = 921600; - } - - _timeout = false; - begin_gmb_uart = true; - _last_time = hal.scheduler->millis(); - _retries++; - if(_retries == MAX_RETRIES) { - hal.console->printf("No MavLink Gimbal Found!!\n"); - } - } else { - if(begin_gmb_uart && ((hal.scheduler->millis() - _last_time)>=1000)) { - uart->begin(baud); - hal.console->printf("Setting Gimbal port baud to %d\n", baud); - begin_gmb_uart = false; - } - _timeout = ((hal.scheduler->millis() - _last_time)>=5000) ? true : false; - } - } - */ } #endif // update each instance