From 87c6d5da13f8ac7236fd4fa0f1fec3e01d3c0b6c Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 21 Nov 2016 18:00:24 -0200 Subject: [PATCH] AP_InertialSensor: replace panic() with return When blocking forever there's no reason to call panic later since it will never going to be reached. This reduces binary size in a few bytes since the message isn't required anymore. --- libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp | 4 ++-- libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp | 4 ++-- libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp index 3942181680..f5305c0cc2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp @@ -153,7 +153,7 @@ void AP_InertialSensor_BMI160::start() bool r; if (!_dev->get_semaphore()->take(0)) { - AP_HAL::panic("BMI160: Unable to get semaphore"); + return; } r = _configure_accel(); @@ -430,7 +430,7 @@ bool AP_InertialSensor_BMI160::_hardware_init() hal.scheduler->delay(BMI160_POWERUP_DELAY_MSEC); if (!_dev->get_semaphore()->take(0)) { - AP_HAL::panic("BMI160: Unable to get semaphore"); + return false; } _dev->set_speed(AP_HAL::Device::SPEED_LOW); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 6de76d4960..dfc8be6bf6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -345,7 +345,7 @@ bool AP_InertialSensor_MPU6000::_has_auxiliary_bus() void AP_InertialSensor_MPU6000::start() { if (!_dev->get_semaphore()->take(0)) { - AP_HAL::panic("MPU6000: Unable to get semaphore"); + return; } // initially run the bus at low speed @@ -723,7 +723,7 @@ bool AP_InertialSensor_MPU6000::_check_whoami(void) bool AP_InertialSensor_MPU6000::_hardware_init(void) { if (!_dev->get_semaphore()->take(0)) { - AP_HAL::panic("MPU6000: Unable to get semaphore"); + return false; } // setup for register checking diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 448af71586..9a9ecebae6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -296,7 +296,7 @@ bool AP_InertialSensor_MPU9250::_has_auxiliary_bus() void AP_InertialSensor_MPU9250::start() { if (!_dev->get_semaphore()->take(0)) { - AP_HAL::panic("MPU92500: Unable to get semaphore"); + return; } // initially run the bus at low speed @@ -601,7 +601,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(0)) { - AP_HAL::panic("MPU9250: Unable to get semaphore"); + return false; } // setup for register checking