mirror of https://github.com/ArduPilot/ardupilot
RangeFinder: Change to coding style (NFC)
RangeFinder: RangeFinder: Change to coding style (NFC)
This commit is contained in:
parent
87a1efeb45
commit
4754710e60
|
@ -65,13 +65,11 @@ bool AP_RangeFinder_BBB_PRU::detect()
|
|||
|
||||
// Load firmware (.text)
|
||||
FILE *file = fopen("/lib/firmware/rangefinderprutext.bin", "rb");
|
||||
if(file == nullptr)
|
||||
{
|
||||
if (file == nullptr) {
|
||||
result = false;
|
||||
}
|
||||
|
||||
if(fread(ram, PRU0_IRAM_SIZE, 1, file) != 1)
|
||||
{
|
||||
if (fread(ram, PRU0_IRAM_SIZE, 1, file) != 1) {
|
||||
result = false;
|
||||
}
|
||||
|
||||
|
@ -83,13 +81,11 @@ bool AP_RangeFinder_BBB_PRU::detect()
|
|||
|
||||
// Load firmware (.data)
|
||||
file = fopen("/lib/firmware/rangefinderprudata.bin", "rb");
|
||||
if(file == nullptr)
|
||||
{
|
||||
if (file == nullptr) {
|
||||
result = false;
|
||||
}
|
||||
|
||||
if(fread(ram, PRU0_DRAM_SIZE, 1, file) != 1)
|
||||
{
|
||||
if (fread(ram, PRU0_DRAM_SIZE, 1, file) != 1) {
|
||||
result = false;
|
||||
}
|
||||
|
||||
|
|
|
@ -121,23 +121,25 @@ unsigned short AP_RangeFinder_Bebop::get_threshold_at(int i_capture)
|
|||
* */
|
||||
switch (_mode) {
|
||||
case 0:
|
||||
if (i_capture < 139)
|
||||
if (i_capture < 139) {
|
||||
threshold_value = 4195;
|
||||
else if (i_capture < 153)
|
||||
] else if (i_capture < 153) {
|
||||
threshold_value = waveform_mode0[i_capture - 139];
|
||||
else
|
||||
} else {
|
||||
threshold_value = 1200;
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if (i_capture < 73)
|
||||
if (i_capture < 73) {
|
||||
threshold_value = 4195;
|
||||
else if (i_capture < 105)
|
||||
} else if (i_capture < 105) {
|
||||
threshold_value = waveform_mode1[i_capture - 73];
|
||||
else if (i_capture < 617)
|
||||
} else if (i_capture < 617) {
|
||||
threshold_value = 1200;
|
||||
else
|
||||
} else {
|
||||
threshold_value = 4195;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
|
@ -171,10 +173,11 @@ int AP_RangeFinder_Bebop::_apply_averaging_filter(void)
|
|||
|
||||
/* We keep on advancing in the captured buffer without registering the
|
||||
* filtered data until the signal first exceeds a given value */
|
||||
if (!first_echo && current_value < threshold_echo_init)
|
||||
if (!first_echo && current_value < threshold_echo_init) {
|
||||
continue;
|
||||
else
|
||||
} else {
|
||||
first_echo = true;
|
||||
}
|
||||
|
||||
filtered_value += current_value;
|
||||
if (i_capture % _filter_average == 0) {
|
||||
|
@ -202,8 +205,9 @@ int AP_RangeFinder_Bebop::_search_local_maxima(void)
|
|||
get_threshold_at(i_capture - 1))) {
|
||||
_echoes[i_echo].max_index = i_capture;
|
||||
i_echo++;
|
||||
if (i_echo >= RNFD_BEBOP_MAX_ECHOES)
|
||||
if (i_echo >= RNFD_BEBOP_MAX_ECHOES) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -224,10 +228,11 @@ int AP_RangeFinder_Bebop::_search_maximum_with_max_amplitude(void)
|
|||
}
|
||||
}
|
||||
|
||||
if (max_idx >= 0)
|
||||
if (max_idx >= 0) {
|
||||
return _echoes[max_idx].max_index;
|
||||
else
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_RangeFinder_Bebop::_loop(void)
|
||||
|
@ -304,10 +309,12 @@ void AP_RangeFinder_Bebop::_reconfigure_wave()
|
|||
{
|
||||
/* configure the output buffer for a purge */
|
||||
/* perform a purge */
|
||||
if (_launch_purge() < 0)
|
||||
if (_launch_purge() < 0) {
|
||||
hal.console->printf("purge could not send data overspi");
|
||||
if (_capture() < 0)
|
||||
}
|
||||
if (_capture() < 0) {
|
||||
hal.console->printf("purge could not capture data");
|
||||
}
|
||||
|
||||
_tx_buf = _tx[_mode];
|
||||
switch (_mode) {
|
||||
|
@ -342,8 +349,9 @@ int AP_RangeFinder_Bebop::_configure_capture()
|
|||
const char *adcchannel = "voltage2";
|
||||
/* configure adc interface using libiio */
|
||||
_iio = iio_create_local_context();
|
||||
if (!_iio)
|
||||
if (!_iio) {
|
||||
return -1;
|
||||
}
|
||||
_adc.device = iio_context_find_device(_iio, adcname);
|
||||
|
||||
if (!_adc.device) {
|
||||
|
|
|
@ -171,8 +171,9 @@ bool AP_RangeFinder_Benewake_TFMiniPlus::check_checksum(uint8_t *arr, int pkt_le
|
|||
int i;
|
||||
|
||||
/* sum them all except the last (the checksum) */
|
||||
for (i = 0; i < pkt_len - 1; i++)
|
||||
for (i = 0; i < pkt_len - 1; i++) {
|
||||
checksum += arr[i];
|
||||
}
|
||||
|
||||
return checksum == arr[pkt_len - 1];
|
||||
}
|
||||
|
|
|
@ -120,10 +120,12 @@ bool AP_RangeFinder_NMEA::decode(char c)
|
|||
}
|
||||
|
||||
// ordinary characters are added to term
|
||||
if (_term_offset < sizeof(_term) - 1)
|
||||
if (_term_offset < sizeof(_term) - 1) {
|
||||
_term[_term_offset++] = c;
|
||||
if (!_term_is_checksum)
|
||||
}
|
||||
if (!_term_is_checksum) {
|
||||
_checksum ^= c;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
|
|
@ -228,7 +228,7 @@ AP_RangeFinder_VL53L0X::AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_
|
|||
*/
|
||||
AP_RangeFinder_Backend *AP_RangeFinder_VL53L0X::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
|
||||
{
|
||||
if(!dev){
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_RangeFinder_VL53L0X *sensor
|
||||
|
|
|
@ -250,7 +250,7 @@ bool AP_RangeFinder_VL53L1X::setMeasurementTimingBudget(uint32_t budget_us)
|
|||
// VL53L1_calc_timeout_register_values() begin
|
||||
|
||||
uint8_t range_config_vcsel_period = 0;
|
||||
if(!read_register(RANGE_CONFIG__VCSEL_PERIOD_A, range_config_vcsel_period)) {
|
||||
if (!read_register(RANGE_CONFIG__VCSEL_PERIOD_A, range_config_vcsel_period)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue