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); }