AP_AHRS: don't allow get_velocity_NED() and get_relative_position_NED() without EKF

this avoids some linking issues, plus the functions are inaccurate
without EKF
This commit is contained in:
Andrew Tridgell 2014-02-08 19:11:12 +11:00
parent ebf6d47830
commit 25ef0d5a7b
5 changed files with 16 additions and 47 deletions

View File

@ -204,12 +204,12 @@ public:
// return a ground velocity in meters/second, North/East/Down
// order. This will only be accurate if have_inertial_nav() is
// true
virtual Vector3f get_velocity_NED(void) = 0;
virtual bool get_velocity_NED(Vector3f &vec) const { return false; }
// return a position relative to home in meters, North/East/Down
// order. This will only be accurate if have_inertial_nav() is
// true
virtual Vector3f get_relative_position_NED(void) = 0;
virtual bool get_relative_position_NED(Vector3f &vec) const { return false; }
// return ground speed estimate in meters/second. Used by ground vehicles.
float groundspeed(void) const {

View File

@ -913,32 +913,3 @@ void AP_AHRS_DCM::set_home(int32_t lat, int32_t lng, int32_t alt_cm)
_home.alt = alt_cm;
}
// return a ground velocity in meters/second, North/East/Down order
Vector3f AP_AHRS_DCM::get_velocity_NED(void)
{
Vector2f xy = groundspeed_vector();
Vector3f vec(xy.x, xy.y, 0);
// if we have GPS lock and GPS has raw velocities then use GPS
// vertical velocity for z
if (_gps && _gps->status() >= GPS::GPS_OK_FIX_3D) {
vec.z = _gps->velocity_down();
} else {
// otherwise we have no choice but the baro climb rate
vec.z = - _baro.get_climb_rate();
}
return vec;
}
// return a relative position in meters, NED order This is only
// approximate. Use the NavEKF version for accurate positioning
Vector3f AP_AHRS_DCM::get_relative_position_NED(void)
{
struct Location loc;
if (!get_position(loc)) {
// not available
return Vector3f(0,0,0);
}
Vector2f xy = location_diff(loc, _home);
return Vector3f(xy.x, xy.y, (_home.alt - loc.alt) * 1.0e-2f);
}

View File

@ -79,10 +79,7 @@ public:
bool use_compass(void);
void set_home(int32_t lat, int32_t lng, int32_t alt_cm);
Vector3f get_velocity_NED(void);
Vector3f get_relative_position_NED(void);
void estimate_wind(void);
void estimate_wind(void);
private:
float _ki;

View File

@ -205,24 +205,25 @@ bool AP_AHRS_NavEKF::have_inertial_nav(void) const
return using_EKF();
}
// return a ground velocity in meters/second, North/East/Down order
Vector3f AP_AHRS_NavEKF::get_velocity_NED(void)
// return a ground velocity in meters/second, North/East/Down
// order. Must only be called if have_inertial_nav() is true
bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
{
if (using_EKF()) {
Vector3f vec;
EKF.getVelNED(vec);
return vec;
return true;
}
return AP_AHRS_DCM::get_velocity_NED();
return false;
}
Vector3f AP_AHRS_NavEKF::get_relative_position_NED(void)
// return a relative ground position in meters/second, North/East/Down
// order. Must only be called if have_inertial_nav() is true
bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const
{
Vector3f ret;
if (using_EKF() && EKF.getPosNED(ret)) {
return ret;
if (using_EKF()) {
return EKF.getPosNED(vec);
}
return AP_AHRS_DCM::get_relative_position_NED();
return false;
}
#endif // AP_AHRS_NAVEKF_AVAILABLE

View File

@ -89,8 +89,8 @@ public:
bool have_inertial_nav(void) const;
Vector3f get_velocity_NED(void);
Vector3f get_relative_position_NED(void);
bool get_velocity_NED(Vector3f &vec) const;
bool get_relative_position_NED(Vector3f &vec) const;
void set_ekf_use(bool setting) { _ekf_use.set(setting); }