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

AP_AHRS: implement get_velocity_NED() for DCM

This commit is contained in:
Andrew Tridgell 2019-03-14 13:51:57 +11:00
parent e014b3d308
commit 2ffded4dd7
3 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -1075,3 +1075,17 @@ bool AP_AHRS_DCM::healthy(void) const
// consider ourselves healthy if there have been no failures for 5 seconds
return (_last_failure_ms == 0 || AP_HAL::millis() - _last_failure_ms > 5000);
}
/*
return NED velocity if we have GPS lock
*/
bool AP_AHRS_DCM::get_velocity_NED(Vector3f &vec) const
{
const AP_GPS &_gps = AP::gps();
if (_gps.status() < AP_GPS::GPS_OK_FIX_3D) {
return false;
}
vec = _gps.velocity();
return true;
}

2
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -102,6 +102,8 @@ public:
// is the AHRS subsystem healthy?
bool healthy() const override;
bool get_velocity_NED(Vector3f &vec) const override;
private:
float _ki;
float _ki_yaw;

2
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -643,7 +643,7 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
return AP_AHRS_DCM::get_velocity_NED(vec);
case EKF_TYPE2:
default: