diff --git a/APMrover2/system.pde b/APMrover2/system.pde index 250e095012..3029b562ab 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -51,6 +51,9 @@ static void run_cli(AP_HAL::UARTDriver *port) // disable the failsafe code in the CLI hal.scheduler->register_timer_failsafe(NULL,1); + // disable the mavlink delay callback + hal.scheduler->register_delay_callback(NULL, 5); + cliSerial = port; Menu::set_port(port); port->set_blocking_writes(true); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 62a2d5d090..f5338ec4ba 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -54,6 +54,9 @@ static void run_cli(AP_HAL::UARTDriver *port) Menu::set_port(port); port->set_blocking_writes(true); + // disable the mavlink delay callback + hal.scheduler->register_delay_callback(NULL, 5); + while (1) { main_menu.run(); } diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index be41cfa606..a304f81de5 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -54,6 +54,9 @@ static void run_cli(AP_HAL::UARTDriver *port) // disable the failsafe code in the CLI hal.scheduler->register_timer_failsafe(NULL,1); + // disable the mavlink delay callback + hal.scheduler->register_delay_callback(NULL, 5); + cliSerial = port; Menu::set_port(port); port->set_blocking_writes(true);