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

added baro_alt_offset back in. Filtering out the effect over time. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1925 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-04-28 05:43:35 +00:00
parent 78510636db
commit bc7c797fd6
2 changed files with 20 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduCopterMega/ArduCopterMega.pde
View File

@ -313,6 +313,7 @@ int ground_temperature;
// ---------------------- // ----------------------
int sonar_alt; int sonar_alt;
int baro_alt; int baro_alt;
int baro_alt_offset;
byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
// flight mode specific // flight mode specific
@ -766,6 +767,11 @@ void slow_loop()
tuning(); tuning();
#endif #endif
// filter out the baro offset.
if(baro_alt_offset > 0) baro_alt_offset--;
if(baro_alt_offset < 0) baro_alt_offset++;
break; break;
default: default:
@ -1165,18 +1171,20 @@ void update_alt()
current_loc.alt = g_gps->altitude; current_loc.alt = g_gps->altitude;
#else #else
if(g.sonar_enabled){ if(g.sonar_enabled){
// filter out offset
// read barometer // read barometer
baro_alt = read_barometer(); baro_alt = read_barometer();
// XXX temp removed fr debugging // XXX temp removed fr debugging
//filter out bad sonar reads //filter out bad sonar reads
//int temp = sonar.read(); int temp = sonar.read();
//if(abs(temp - sonar_alt) < 300){ if(abs(temp - sonar_alt) < 300){
// sonar_alt = temp; sonar_alt = temp;
//} }
sonar_alt = sonar.read(); //sonar_alt = sonar.read();
// output a light to show sonar is working // output a light to show sonar is working
update_sonar_light(sonar_alt > 100); update_sonar_light(sonar_alt > 100);
@ -1192,12 +1200,15 @@ void update_alt()
if(sonar_alt < 400){ if(sonar_alt < 400){
altitude_sensor = SONAR; altitude_sensor = SONAR;
baro_alt_offset = sonar_alt - baro_alt;
} }
} }
// calculate our altitude // calculate our altitude
if(altitude_sensor == BARO){ if(altitude_sensor == BARO){
current_loc.alt = baro_alt + home.alt; current_loc.alt = baro_alt + baro_alt_offset + home.alt;
}else{ }else{
current_loc.alt = sonar_alt + home.alt; current_loc.alt = sonar_alt + home.alt;
} }

6
ArduCopterMega/config.h
View File

@ -390,7 +390,7 @@
# define NAV_WP_D 15 // not sure about at all # define NAV_WP_D 15 // not sure about at all
#endif #endif
#ifndef NAV_WP_IMAX #ifndef NAV_WP_IMAX
# define NAV_WP_IMAX 20 // 20 degrees # define NAV_WP_IMAX 30 // 20 degrees
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -406,7 +406,7 @@
# define THROTTLE_BARO_D 0.1 # define THROTTLE_BARO_D 0.1
#endif #endif
#ifndef THROTTLE_BARO_IMAX #ifndef THROTTLE_BARO_IMAX
# define THROTTLE_BARO_IMAX 80 # define THROTTLE_BARO_IMAX 50
#endif #endif
@ -420,7 +420,7 @@
# define THROTTLE_SONAR_D 0.1 # define THROTTLE_SONAR_D 0.1
#endif #endif
#ifndef THROTTLE_SONAR_IMAX #ifndef THROTTLE_SONAR_IMAX
# define THROTTLE_SONAR_IMAX 80 # define THROTTLE_SONAR_IMAX 60
#endif #endif