mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_RangeFinder: Ensure no divide by zeros in LeddarOne
Having a critical divide by value as a class member that could change external to the function using it is fragile. It was not very obvious that a divide by zero was not possible in the current design, now it's very obvious and safer in case the code changes later.
This commit is contained in:
parent
b6867e085d
commit
e928e20b11
@ -68,12 +68,16 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
|
||||
}
|
||||
|
||||
// parse a response message, set detections and sum_distance
|
||||
if (parse_response() <= 0) {
|
||||
// must be signed to handle errors
|
||||
int8_t number_detections = parse_response();
|
||||
|
||||
if (number_detections <= 0) {
|
||||
// TODO: when (number_detections < 0) handle LEDDARONE_ERR_
|
||||
return false;
|
||||
}
|
||||
|
||||
// calculate average distance
|
||||
reading_cm = sum_distance / number_detections;
|
||||
reading_cm = sum_distance / (uint8_t)number_detections;
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -204,7 +208,7 @@ int8_t AP_RangeFinder_LeddarOne::parse_response(void)
|
||||
}
|
||||
|
||||
// number of detections
|
||||
number_detections = data_buffer[10];
|
||||
uint8_t number_detections = data_buffer[10];
|
||||
|
||||
// if the number of detection is over , it is false
|
||||
if (number_detections > (sizeof(detections) / sizeof(detections[0]))) {
|
||||
@ -220,5 +224,5 @@ int8_t AP_RangeFinder_LeddarOne::parse_response(void)
|
||||
index_offset += 4;
|
||||
}
|
||||
|
||||
return number_detections;
|
||||
return (int8_t)number_detections;
|
||||
}
|
||||
|
@ -47,6 +47,5 @@ private:
|
||||
uint32_t last_reading_ms = 0;
|
||||
|
||||
uint16_t detections[3];
|
||||
uint8_t number_detections = 0;
|
||||
uint32_t sum_distance = 0;
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user