Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: replace range finder health with status

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)
{
if (!sonar.healthy()) {
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 more than one enabled
report smaller distance of two sonars
*/
float distance_cm, voltage;
if (!sonar.healthy(1)) {
distance_cm = sonar.distance_cm(0);
voltage = sonar.voltage_mv(0) * 0.001f;
} 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;
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 = 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;
}
}
mavlink_msg_rangefinder_send(
chan,
distance_cm * 0.01f,

4
APMrover2/sensors.pde
View File

@ -33,12 +33,12 @@ static void read_sonars(void)
{
sonar.update();
if (!sonar.healthy()) {
if (sonar.status() == RangeFinder::RangeFinder_NotConnected) {
// this makes it possible to disable sonar at runtime
return;
}
if (sonar.healthy(1)) {
if (sonar.has_data(1)) {
// we have two sonars
obstacle.sonar1_distance_cm = sonar.distance_cm(0);
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);
sonar.update();
if (!sonar.healthy()) {
if (sonar.status() == RangeFinder::RangeFinder_NotConnected) {
cliSerial->println_P(PSTR("WARNING: Sonar is not enabled"));
}