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

AP_InertialNav: Add output method for rate of change of vertical position

This commit is contained in:
Paul Riseborough 2015-10-12 19:13:37 +11:00 committed by Andrew Tridgell
parent dab658f6ed
commit f062ed4180
2 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
View File

@ -24,9 +24,15 @@ void AP_InertialNav_NavEKF::update(float dt)
_ahrs_ekf.get_NavEKF().getVelNED(_velocity_cm); _ahrs_ekf.get_NavEKF().getVelNED(_velocity_cm);
_velocity_cm *= 100; // convert to cm/s _velocity_cm *= 100; // convert to cm/s
// A derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
// This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
_ahrs_ekf.get_NavEKF().getPosDownDerivative(_pos_z_rate);
_pos_z_rate *= 100; // convert to cm/s
// InertialNav is NEU // InertialNav is NEU
_relpos_cm.z = - _relpos_cm.z; _relpos_cm.z = - _relpos_cm.z;
_velocity_cm.z = -_velocity_cm.z; _velocity_cm.z = -_velocity_cm.z;
_pos_z_rate = - _pos_z_rate;
} }
/** /**
@ -111,6 +117,14 @@ float AP_InertialNav_NavEKF::get_velocity_xy() const
return pythagorous2(_velocity_cm.x, _velocity_cm.y); return pythagorous2(_velocity_cm.x, _velocity_cm.y);
} }
/**
* get_pos_z_derivative - returns the derivative of the z position in cm/s
*/
float AP_InertialNav_NavEKF::get_pos_z_derivative() const
{
return _pos_z_rate;
}
/** /**
* get_altitude - get latest altitude estimate in cm * get_altitude - get latest altitude estimate in cm
* @return * @return

6
libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
View File

@ -75,6 +75,11 @@ public:
*/ */
const Vector3f& get_velocity() const; const Vector3f& get_velocity() const;
/**
* get_pos_z_derivative - returns the derivative of the z position in cm/s
*/
float get_pos_z_derivative() const;
/** /**
* get_velocity_xy - returns the current horizontal velocity in cm/s * get_velocity_xy - returns the current horizontal velocity in cm/s
* *
@ -114,6 +119,7 @@ public:
private: private:
Vector3f _relpos_cm; // NEU Vector3f _relpos_cm; // NEU
Vector3f _velocity_cm; // NEU Vector3f _velocity_cm; // NEU
float _pos_z_rate;
struct Location _abspos; struct Location _abspos;
bool _haveabspos; bool _haveabspos;
AP_AHRS_NavEKF &_ahrs_ekf; AP_AHRS_NavEKF &_ahrs_ekf;