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

Added better navigation flag for more accurate dtnav timing 

Added better support for climb rate in SIM
This commit is contained in:
Jason Short 2011-11-14 16:43:30 -08:00 committed by Jason Short
parent 1caa25a7dd
commit d475e7ced5
1 changed files with 27 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduCopter/ArduCopter.pde
View File

@ -299,6 +299,7 @@ static bool did_ground_start = false; // have we ground started after first ar
// Location & Navigation // Location & Navigation
// --------------------- // ---------------------
static bool nav_ok;
static const float radius_of_earth = 6378100; // meters static const float radius_of_earth = 6378100; // meters
static const float gravity = 9.81; // meters/ sec^2 static const float gravity = 9.81; // meters/ sec^2
static int32_t target_bearing; // deg * 100 : 0 to 360 location of the plane to the target static int32_t target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
@ -640,18 +641,14 @@ static void medium_loop()
medium_loopCounter++; medium_loopCounter++;
// Auto control modes: // Auto control modes:
if(g_gps->new_data && g_gps->fix){ if(nav_ok){
// clear nav flag
nav_ok = false;
// invalidate GPS data // invalidate GPS data
// -------------------
g_gps->new_data = false; g_gps->new_data = false;
// we are not tracking I term on navigation, so this isn't needed
dTnav = (float)(millis() - nav_loopTimer)/ 1000.0;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = min(dTnav, 1.0);
// calculate the copter's desired bearing and WP distance // calculate the copter's desired bearing and WP distance
// ------------------------------------------------------ // ------------------------------------------------------
if(navigate()){ if(navigate()){
@ -663,10 +660,7 @@ static void medium_loop()
if (g.log_bitmask & MASK_LOG_NTUN) if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning(); Log_Write_Nav_Tuning();
} }
}else{
g_gps->new_data = false;
} }
break; break;
// command processing // command processing
@ -900,18 +894,27 @@ static void update_GPS(void)
gps_watchdog++; gps_watchdog++;
}else{ }else{
// we have lost GPS signal for a moment. Reduce our error to avoid flyaways // we have lost GPS signal for a moment. Reduce our error to avoid flyaways
// commented temporarily nav_roll >>= 1;
//nav_roll >>= 1; nav_pitch >>= 1;
//nav_pitch >>= 1;
} }
if (g_gps->new_data && g_gps->fix) { if (g_gps->new_data && g_gps->fix) {
gps_watchdog = 0; gps_watchdog = 0;
// OK to run the nav routines
nav_ok = true;
// for performance // for performance
// --------------- // ---------------
gps_fix_count++; gps_fix_count++;
// we are not tracking I term on navigation, so this isn't needed
dTnav = (float)(millis() - nav_loopTimer)/ 1000.0;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = min(dTnav, 1.0);
if(ground_start_count > 1){ if(ground_start_count > 1){
ground_start_count--; ground_start_count--;
@ -935,11 +938,14 @@ static void update_GPS(void)
if (g.log_bitmask & MASK_LOG_GPS){ if (g.log_bitmask & MASK_LOG_GPS){
Log_Write_GPS(); Log_Write_GPS();
} }
}
#if HIL_MODE == HIL_MODE_ATTITUDE // don't execute in HIL mode #if HIL_MODE == HIL_MODE_ATTITUDE // only execute in HIL mode
update_altitude(); update_altitude();
#endif #endif
} else {
g_gps->new_data = false;
}
} }
@ -1226,12 +1232,12 @@ static void update_altitude()
#if HIL_MODE == HIL_MODE_ATTITUDE #if HIL_MODE == HIL_MODE_ATTITUDE
// we are in the SIM, fake out the baro and Sonar // we are in the SIM, fake out the baro and Sonar
int fake_relative_alt = g_gps->altitude - gps_base_alt; int fake_relative_alt = g_gps->altitude - gps_base_alt;
int temp_baro_alt = fake_relative_alt;
baro_rate = (temp_baro_alt - old_baro_alt) * 10;
old_baro_alt = temp_baro_alt;
baro_alt = fake_relative_alt; baro_alt = fake_relative_alt;
sonar_alt = fake_relative_alt; sonar_alt = fake_relative_alt;
baro_rate = (baro_alt - old_baro_alt) * 5; // 5hz
old_baro_alt = baro_alt;
#else #else
// This is real life // This is real life
// calc the vertical accel rate // calc the vertical accel rate