Browse Source

AP_RangeFinder: Modified uLanding parse to check device version.

US-D1 version ID was changed to 2 from 0. This alteration will prevent
past US-D1s, with version ID 0, from being 3-byte parsed.
master
alancaro2013 6 years ago committed by Andrew Tridgell
parent
commit
a1557184d5
  1. 4
      libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp

4
libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp

@ -174,7 +174,7 @@ bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm) @@ -174,7 +174,7 @@ bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm)
*/
continue;
} else {
if (_version == 0) {
if (_version == 0 && _header != ULANDING_HDR) {
// parse data for Firmware Version #0
sum += (_linebuf[2]&0x7F)*128 + (_linebuf[1]&0x7F);
count++;
@ -199,7 +199,7 @@ bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm) @@ -199,7 +199,7 @@ bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm)
reading_cm = sum / count;
if (_version == 0) {
if (_version == 0 && _header != ULANDING_HDR) {
reading_cm *= 2.5f;
}

Loading…
Cancel
Save