void Rover::send_rangefinder(mavlink_channel_t chan)
{
float distance_cm;
float voltage;
bool got_one = false;
// report smaller distance of all rangefinders
for (uint8_t i=0; i<rangefinder.num_sensors(); i++) {
AP_RangeFinder_Backend *s = rangefinder.get_backend(i);
if (s == nullptr) {
continue;
}
if (!got_one ||
s->distance_cm() < distance_cm) {
distance_cm = s->distance_cm();
voltage = s->voltage_mv();
got_one = true;
}
}
if (!got_one) {
// no relevant data found
return;
}
mavlink_msg_rangefinder_send(
chan,
distance_cm * 0.01f,
voltage);
}
void Rover::send_rangefinder(mavlink_channel_t chan)
{
if (!sonar.has_data(0) && !sonar.has_data(1)) {
// no sonar to report
return;
}
float distance_cm = 0.0f;
float voltage = 0.0f;
/*
report smaller distance of two sonars
*/
if (sonar.has_data(0) && sonar.has_data(1)) {
if (sonar.distance_cm(0) <= sonar.distance_cm(1)) {
distance_cm = sonar.distance_cm(0);
voltage = sonar.voltage_mv(0);
} else {
distance_cm = sonar.distance_cm(1);
voltage = sonar.voltage_mv(1);
}
} else {
// only sonar 0 or sonar 1 has data
if (sonar.has_data(0)) {
distance_cm = sonar.distance_cm(0);
voltage = sonar.voltage_mv(0) * 0.001f;
}
if (sonar.has_data(1)) {
distance_cm = sonar.distance_cm(1);
voltage = sonar.voltage_mv(1) * 0.001f;
}
}
mavlink_msg_rangefinder_send(
chan,
distance_cm * 0.01f,
voltage);
}
