Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: bug fix to set sonar_alt_health to zero when disabled

Browse Source
This commit is contained in:
Randy Mackay 2013-01-30 21:17:43 +09:00
parent 5b50f31b8f
commit 21ff3853f1
2 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/ArduCopter.pde
View File

@ -2056,8 +2056,10 @@ static void update_altitude()
baro_rate = constrain(baro_rate, -500, 500); baro_rate = constrain(baro_rate, -500, 500);
// read in sonar altitude and calculate sonar rate // read in sonar altitude and calculate sonar rate
if(g.sonar_enabled) { sonar_alt = read_sonar();
sonar_alt = read_sonar(); // start calculating the sonar_rate as soon as valid sonar readings start coming in so that we are ready when the sonar_alt_health becomes 3
// Note: post 2.9.1 release we will remove the sonar_rate variable completely
if(sonar_alt_health > 1) {
sonar_rate = (sonar_alt - old_sonar_alt) * 10; sonar_rate = (sonar_alt - old_sonar_alt) * 10;
sonar_rate = constrain(sonar_rate, -150, 150); sonar_rate = constrain(sonar_rate, -150, 150);
} }

6
ArduCopter/sensors.pde
View File

@ -35,6 +35,12 @@ static int32_t read_barometer(void)
static int16_t read_sonar(void) static int16_t read_sonar(void)
{ {
#if CONFIG_SONAR == ENABLED #if CONFIG_SONAR == ENABLED
// exit immediately if sonar is disabled
if( !g.sonar_enabled ) {
sonar_alt_health = 0;
return 0;
}
int16_t temp_alt = sonar.read(); int16_t temp_alt = sonar.read();
if(temp_alt >= sonar.min_distance && temp_alt <= sonar.max_distance * 0.70) { if(temp_alt >= sonar.min_distance && temp_alt <= sonar.max_distance * 0.70) {