AP_Math: fixed build error on PX4

variable set but not used with recent longitude_scale change
This commit is contained in:
Andrew Tridgell 2015-07-06 08:19:57 +10:00
parent 099392d3ca
commit 8f41d97548
1 changed files with 5 additions and 2 deletions

View File

@ -35,9 +35,9 @@
float longitude_scale(const struct Location &loc)
{
#if HAL_CPU_CLASS < HAL_CPU_CLASS_150
static int32_t last_lat;
static float scale = 1.0;
#if HAL_CPU_CLASS < HAL_CPU_CLASS_150
// don't optimise on faster CPUs. It causes some minor errors on Replay
if (labs(last_lat - loc.lat) < 100000) {
// we are within 0.01 degrees (about 1km) of the
@ -45,11 +45,14 @@ float longitude_scale(const struct Location &loc)
// the same scale factor.
return scale;
}
#endif
scale = cosf(loc.lat * 1.0e-7f * DEG_TO_RAD);
scale = constrain_float(scale, 0.01f, 1.0f);
last_lat = loc.lat;
return scale;
#else
float scale = cosf(loc.lat * 1.0e-7f * DEG_TO_RAD);
return constrain_float(scale, 0.01f, 1.0f);
#endif
}