Browse Source

AP_RangeFinder: use single precision string to float

c415-sdk
Andrew Tridgell 5 years ago
parent
commit
5898dc757c
  1. 2
      libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
  2. 4
      libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp
  3. 2
      libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp

2
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp

@ -67,7 +67,7 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm) @@ -67,7 +67,7 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
char c = uart->read();
if (c == '\r') {
linebuf[linebuf_len] = 0;
const float dist = (float)atof(linebuf);
const float dist = strtof(linebuf, NULL);
if (!is_negative(dist)) {
sum += dist;
valid_count++;

4
libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp

@ -170,12 +170,12 @@ bool AP_RangeFinder_NMEA::decode_latest_term() @@ -170,12 +170,12 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
if (_sentence_type == SONAR_DBT) {
// parse DBT messages
if (_term_number == 3) {
_distance_m = atof(_term);
_distance_m = strtof(_term, NULL);
}
} else if (_sentence_type == SONAR_DPT) {
// parse DPT messages
if (_term_number == 1) {
_distance_m = atof(_term);
_distance_m = strtof(_term, NULL);
}
}

2
libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp

@ -108,7 +108,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) { @@ -108,7 +108,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) {
if (isalpha(linebuf[0])) {
parse_response();
} else {
float read_value = (float)atof(linebuf);
float read_value = strtof(linebuf, NULL);
if (read_value > 0) {
sum += read_value;
count++;

Loading…
Cancel
Save