Browse Source

Rover: use rangefinder distance() rather than distance_cm

apm_2208
Peter Barker 3 years ago committed by Andrew Tridgell
parent
commit
29785dabca
  1. 8
      Rover/GCS_Mavlink.cpp

8
Rover/GCS_Mavlink.cpp

@ -149,7 +149,7 @@ int16_t GCS_MAVLINK_Rover::vfr_hud_throttle() const
void GCS_MAVLINK_Rover::send_rangefinder() const void GCS_MAVLINK_Rover::send_rangefinder() const
{ {
float distance_cm; float distance;
float voltage; float voltage;
bool got_one = false; bool got_one = false;
@ -160,8 +160,8 @@ void GCS_MAVLINK_Rover::send_rangefinder() const
continue; continue;
} }
if (!got_one || if (!got_one ||
s->distance_cm() < distance_cm) { s->distance() < distance) {
distance_cm = s->distance_cm(); distance = s->distance();
voltage = s->voltage_mv(); voltage = s->voltage_mv();
got_one = true; got_one = true;
} }
@ -173,7 +173,7 @@ void GCS_MAVLINK_Rover::send_rangefinder() const
mavlink_msg_rangefinder_send( mavlink_msg_rangefinder_send(
chan, chan,
distance_cm * 0.01f, distance,
voltage); voltage);
} }

Loading…
Cancel
Save