aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: added range of sonar distances and voltages

Browse Source
This commit is contained in:
Andrew Tridgell 2013-03-29 08:07:24 +11:00
parent de02f98d28
commit a25fede6bb
1 changed files with 48 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
APMrover2/test.pde
View File

@ -556,16 +556,57 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
print_hit_enter();
init_sonar();
float sonar_dist_cm_min = 0.0f;
float sonar_dist_cm_max = 0.0f;
float voltage_min=0.0f, voltage_max = 0.0f;
float sonar2_dist_cm_min = 0.0f;
float sonar2_dist_cm_max = 0.0f;
float voltage2_min=0.0f, voltage2_max = 0.0f;
uint32_t last_print = 0;
while (true) {
delay(200);
float sonar_dist_cm = sonar.distance_cm();
delay(20);
uint32_t now = millis();
float dist_cm = sonar.distance_cm();
float voltage = sonar.voltage();
float sonar2_dist_cm = sonar2.distance_cm();
float voltage2 = sonar2.voltage();
cliSerial->printf_P(PSTR("sonar1 dist=%5.1fcm volt1=%5.2f sonar2 dist=%5.1fcm volt2=%5.2f\n"),
sonar_dist_cm, voltage,
sonar2_dist_cm, voltage2);
if (sonar_dist_cm_min == 0.0f) {
sonar_dist_cm_min = dist_cm;
voltage_min = voltage;
}
sonar_dist_cm_max = max(sonar_dist_cm_max, dist_cm);
sonar_dist_cm_min = min(sonar_dist_cm_min, dist_cm);
voltage_min = min(voltage_min, voltage);
voltage_max = max(voltage_max, voltage);
dist_cm = sonar2.distance_cm();
voltage = sonar2.voltage();
if (sonar2_dist_cm_min == 0.0f) {
sonar2_dist_cm_min = dist_cm;
voltage2_min = voltage;
}
sonar2_dist_cm_max = max(sonar2_dist_cm_max, dist_cm);
sonar2_dist_cm_min = min(sonar2_dist_cm_min, dist_cm);
voltage2_min = min(voltage2_min, voltage);
voltage2_max = max(voltage2_max, voltage);
if (now - last_print >= 200) {
cliSerial->printf_P(PSTR("sonar1 dist=%.1f:%.1fcm volt1=%.2f:%.2f sonar2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n"),
sonar_dist_cm_min,
sonar_dist_cm_max,
voltage_min,
voltage_max,
sonar2_dist_cm_min,
sonar2_dist_cm_max,
voltage2_min,
voltage2_max);
voltage_min = voltage_max = 0.0f;
voltage2_min = voltage2_max = 0.0f;
sonar_dist_cm_min = sonar_dist_cm_max = 0.0f;
sonar2_dist_cm_min = sonar2_dist_cm_max = 0.0f;
last_print = now;
}
if (cliSerial->available() > 0) {
break;
}