mirror of https://github.com/ArduPilot/ardupilot
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.
This commit is contained in:
parent
c2125a0078
commit
87c6d5da13
|
@ -153,7 +153,7 @@ void AP_InertialSensor_BMI160::start()
|
||||||
bool r;
|
bool r;
|
||||||
|
|
||||||
if (!_dev->get_semaphore()->take(0)) {
|
if (!_dev->get_semaphore()->take(0)) {
|
||||||
AP_HAL::panic("BMI160: Unable to get semaphore");
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
r = _configure_accel();
|
r = _configure_accel();
|
||||||
|
@ -430,7 +430,7 @@ bool AP_InertialSensor_BMI160::_hardware_init()
|
||||||
hal.scheduler->delay(BMI160_POWERUP_DELAY_MSEC);
|
hal.scheduler->delay(BMI160_POWERUP_DELAY_MSEC);
|
||||||
|
|
||||||
if (!_dev->get_semaphore()->take(0)) {
|
if (!_dev->get_semaphore()->take(0)) {
|
||||||
AP_HAL::panic("BMI160: Unable to get semaphore");
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
||||||
|
|
|
@ -345,7 +345,7 @@ bool AP_InertialSensor_MPU6000::_has_auxiliary_bus()
|
||||||
void AP_InertialSensor_MPU6000::start()
|
void AP_InertialSensor_MPU6000::start()
|
||||||
{
|
{
|
||||||
if (!_dev->get_semaphore()->take(0)) {
|
if (!_dev->get_semaphore()->take(0)) {
|
||||||
AP_HAL::panic("MPU6000: Unable to get semaphore");
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// initially run the bus at low speed
|
// 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)
|
bool AP_InertialSensor_MPU6000::_hardware_init(void)
|
||||||
{
|
{
|
||||||
if (!_dev->get_semaphore()->take(0)) {
|
if (!_dev->get_semaphore()->take(0)) {
|
||||||
AP_HAL::panic("MPU6000: Unable to get semaphore");
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// setup for register checking
|
// setup for register checking
|
||||||
|
|
|
@ -296,7 +296,7 @@ bool AP_InertialSensor_MPU9250::_has_auxiliary_bus()
|
||||||
void AP_InertialSensor_MPU9250::start()
|
void AP_InertialSensor_MPU9250::start()
|
||||||
{
|
{
|
||||||
if (!_dev->get_semaphore()->take(0)) {
|
if (!_dev->get_semaphore()->take(0)) {
|
||||||
AP_HAL::panic("MPU92500: Unable to get semaphore");
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// initially run the bus at low speed
|
// 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)
|
bool AP_InertialSensor_MPU9250::_hardware_init(void)
|
||||||
{
|
{
|
||||||
if (!_dev->get_semaphore()->take(0)) {
|
if (!_dev->get_semaphore()->take(0)) {
|
||||||
AP_HAL::panic("MPU9250: Unable to get semaphore");
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// setup for register checking
|
// setup for register checking
|
||||||
|
|
Loading…
Reference in New Issue