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:
parent
ebf6d47830
commit
25ef0d5a7b
@ -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 {
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
@ -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); }
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user