Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: replace range finder health with status

Browse Source
This commit is contained in:
Randy Mackay 2015-04-17 15:41:00 +09:00
parent 5e359c977f
commit 245e46205a
3 changed files with 24 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
APMrover2/GCS_Mavlink.pde
View File

@ -327,29 +327,37 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
static void NOINLINE send_rangefinder(mavlink_channel_t chan) static void NOINLINE send_rangefinder(mavlink_channel_t chan)
{ {
if (!sonar.healthy()) { if (!sonar.has_data(0) && !sonar.has_data(1)) {
// no sonar to report // no sonar to report
return; return;
} }
float distance_cm = 0.0f;
float voltage = 0.0f;
/* /*
report smaller distance of two sonars if more than one enabled report smaller distance of two sonars
*/ */
float distance_cm, voltage; if (sonar.has_data(0) && sonar.has_data(1)) {
if (!sonar.healthy(1)) { if (sonar.distance_cm(0) <= sonar.distance_cm(1)) {
distance_cm = sonar.distance_cm(0); distance_cm = sonar.distance_cm(0);
voltage = sonar.voltage_mv(0) * 0.001f; voltage = sonar.voltage_mv(0);
} else {
float dist1 = sonar.distance_cm(0);
float dist2 = sonar.distance_cm(1);
if (dist1 <= dist2) {
distance_cm = dist1;
voltage = sonar.voltage_mv(0) * 0.001f;
} else { } else {
distance_cm = dist2; 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; voltage = sonar.voltage_mv(1) * 0.001f;
} }
} }
mavlink_msg_rangefinder_send( mavlink_msg_rangefinder_send(
chan, chan,
distance_cm * 0.01f, distance_cm * 0.01f,

4
APMrover2/sensors.pde
View File

@ -33,12 +33,12 @@ static void read_sonars(void)
{ {
sonar.update(); sonar.update();
if (!sonar.healthy()) { if (sonar.status() == RangeFinder::RangeFinder_NotConnected) {
// this makes it possible to disable sonar at runtime // this makes it possible to disable sonar at runtime
return; return;
} }
if (sonar.healthy(1)) { if (sonar.has_data(1)) {
// we have two sonars // we have two sonars
obstacle.sonar1_distance_cm = sonar.distance_cm(0); obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = sonar.distance_cm(1); obstacle.sonar2_distance_cm = sonar.distance_cm(1);

2
APMrover2/test.pde
View File

@ -459,7 +459,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
delay(20); delay(20);
sonar.update(); sonar.update();
if (!sonar.healthy()) { if (sonar.status() == RangeFinder::RangeFinder_NotConnected) {
cliSerial->println_P(PSTR("WARNING: Sonar is not enabled")); cliSerial->println_P(PSTR("WARNING: Sonar is not enabled"));
} }