From 6129b1abb66718a381e1f300d11194f92e9e3bdb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Nov 2016 12:04:22 +1100 Subject: [PATCH] AP_InertialSensor: wait forever for semaphore on startup some boards take a silly amount of time to get semaphore on startup --- libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp | 4 ++-- libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp | 6 +++--- libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp index 42249ff09b..3942181680 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp @@ -152,7 +152,7 @@ void AP_InertialSensor_BMI160::start() { bool r; - if (!_dev->get_semaphore()->take(100)) { + if (!_dev->get_semaphore()->take(0)) { AP_HAL::panic("BMI160: Unable to get semaphore"); } @@ -429,7 +429,7 @@ bool AP_InertialSensor_BMI160::_hardware_init() hal.scheduler->delay(BMI160_POWERUP_DELAY_MSEC); - if (!_dev->get_semaphore()->take(100)) { + if (!_dev->get_semaphore()->take(0)) { AP_HAL::panic("BMI160: Unable to get semaphore"); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 8b53209fcf..6de76d4960 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -344,7 +344,7 @@ bool AP_InertialSensor_MPU6000::_has_auxiliary_bus() void AP_InertialSensor_MPU6000::start() { - if (!_dev->get_semaphore()->take(100)) { + if (!_dev->get_semaphore()->take(0)) { AP_HAL::panic("MPU6000: Unable to get semaphore"); } @@ -722,7 +722,7 @@ bool AP_InertialSensor_MPU6000::_check_whoami(void) bool AP_InertialSensor_MPU6000::_hardware_init(void) { - if (!_dev->get_semaphore()->take(100)) { + if (!_dev->get_semaphore()->take(0)) { AP_HAL::panic("MPU6000: Unable to get semaphore"); } @@ -803,7 +803,7 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void) void AP_InertialSensor_MPU6000::_dump_registers(void) { hal.console->println("MPU6000 registers"); - if (!_dev->get_semaphore()->take(100)) { + if (!_dev->get_semaphore()->take(0)) { return; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 82927e8bd6..448af71586 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -295,7 +295,7 @@ bool AP_InertialSensor_MPU9250::_has_auxiliary_bus() void AP_InertialSensor_MPU9250::start() { - if (!_dev->get_semaphore()->take(100)) { + if (!_dev->get_semaphore()->take(0)) { AP_HAL::panic("MPU92500: Unable to get semaphore"); } @@ -600,7 +600,7 @@ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val, bool c bool AP_InertialSensor_MPU9250::_hardware_init(void) { - if (!_dev->get_semaphore()->take(100)) { + if (!_dev->get_semaphore()->take(0)) { AP_HAL::panic("MPU9250: Unable to get semaphore"); }