Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: const accessors

Browse Source
This commit is contained in:
Peter Barker 2018-04-06 23:14:16 +10:00 committed by Randy Mackay
parent 6791808ac6
commit a0d874baa2
2 changed files with 46 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -816,7 +816,7 @@ int8_t NavEKF3::getPrimaryCoreIMUIndex(void) const
// Write the last calculated NE position relative to the reference point (m).
// If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control
bool NavEKF3::getPosNE(int8_t instance, Vector2f &posNE)
bool NavEKF3::getPosNE(int8_t instance, Vector2f &posNE) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (!core) {
@ -828,7 +828,7 @@ bool NavEKF3::getPosNE(int8_t instance, Vector2f &posNE)
// Write the last calculated D position relative to the reference point (m).
// If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control
bool NavEKF3::getPosD(int8_t instance, float &posD)
bool NavEKF3::getPosD(int8_t instance, float &posD) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (!core) {
@ -838,7 +838,7 @@ bool NavEKF3::getPosD(int8_t instance, float &posD)
}
// return NED velocity in m/s
void NavEKF3::getVelNED(int8_t instance, Vector3f &vel)
void NavEKF3::getVelNED(int8_t instance, Vector3f &vel) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -847,7 +847,7 @@ void NavEKF3::getVelNED(int8_t instance, Vector3f &vel)
}
// Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
float NavEKF3::getPosDownDerivative(int8_t instance)
float NavEKF3::getPosDownDerivative(int8_t instance) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
// return the value calculated from a complementary filer applied to the EKF height and vertical acceleration
@ -866,7 +866,7 @@ void NavEKF3::getAccelNED(Vector3f &accelNED) const
}
// return body axis gyro bias estimates in rad/sec
void NavEKF3::getGyroBias(int8_t instance, Vector3f &gyroBias)
void NavEKF3::getGyroBias(int8_t instance, Vector3f &gyroBias) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -875,7 +875,7 @@ void NavEKF3::getGyroBias(int8_t instance, Vector3f &gyroBias)
}
// return accelerometer bias estimate in m/s/s
void NavEKF3::getAccelBias(int8_t instance, Vector3f &accelBias)
void NavEKF3::getAccelBias(int8_t instance, Vector3f &accelBias) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -884,7 +884,7 @@ void NavEKF3::getAccelBias(int8_t instance, Vector3f &accelBias)
}
// return tilt error convergence metric for the specified instance
void NavEKF3::getTiltError(int8_t instance, float &ang)
void NavEKF3::getTiltError(int8_t instance, float &ang) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -944,7 +944,7 @@ void NavEKF3::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca
}
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void NavEKF3::getWind(int8_t instance, Vector3f &wind)
void NavEKF3::getWind(int8_t instance, Vector3f &wind) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -953,7 +953,7 @@ void NavEKF3::getWind(int8_t instance, Vector3f &wind)
}
// return earth magnetic field estimates in measurement units / 1000
void NavEKF3::getMagNED(int8_t instance, Vector3f &magNED)
void NavEKF3::getMagNED(int8_t instance, Vector3f &magNED) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -962,7 +962,7 @@ void NavEKF3::getMagNED(int8_t instance, Vector3f &magNED)
}
// return body magnetic field estimates in measurement units / 1000
void NavEKF3::getMagXYZ(int8_t instance, Vector3f &magXYZ)
void NavEKF3::getMagXYZ(int8_t instance, Vector3f &magXYZ) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -971,7 +971,7 @@ void NavEKF3::getMagXYZ(int8_t instance, Vector3f &magXYZ)
}
// return the magnetometer in use for the specified instance
uint8_t NavEKF3::getActiveMag(int8_t instance)
uint8_t NavEKF3::getActiveMag(int8_t instance) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1048,7 +1048,7 @@ bool NavEKF3::getHAGL(float &HAGL) const
}
// return the Euler roll, pitch and yaw angle in radians for the specified instance
void NavEKF3::getEulerAngles(int8_t instance, Vector3f &eulers)
void NavEKF3::getEulerAngles(int8_t instance, Vector3f &eulers) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1074,7 +1074,7 @@ void NavEKF3::getQuaternion(int8_t instance, Quaternion &quat) const
}
// return the innovations for the specified instance
void NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov)
void NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1092,7 +1092,7 @@ void NavEKF3::getOutputTrackingError(int8_t instance, Vector3f &error) const
}
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset)
void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1101,7 +1101,7 @@ void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float
}
// return the diagonals from the covariance matrix for the specified instance
void NavEKF3::getStateVariances(int8_t instance, float stateVar[24])
void NavEKF3::getStateVariances(int8_t instance, float stateVar[24]) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1137,7 +1137,7 @@ void NavEKF3::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates,
// return data for debugging optical flow fusion
void NavEKF3::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov,
float &HAGL, float &rngInnov, float &range, float &gndOffsetErr)
float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1182,7 +1182,7 @@ void NavEKF3::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms,
}
// return data for debugging body frame odometry fusion
uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar)
uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar) const
{
uint32_t ret = 0;
if (instance < 0 || instance >= num_cores) {
@ -1198,7 +1198,7 @@ uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vec
// return data for debugging range beacon fusion
bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED)
float &offsetHigh, float &offsetLow, Vector3f &posNED) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1254,7 +1254,7 @@ void NavEKF3::setTerrainHgtStable(bool val)
7 = badly conditioned synthetic sideslip fusion
7 = filter is not initialised
*/
void NavEKF3::getFilterFaults(int8_t instance, uint16_t &faults)
void NavEKF3::getFilterFaults(int8_t instance, uint16_t &faults) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1275,7 +1275,7 @@ void NavEKF3::getFilterFaults(int8_t instance, uint16_t &faults)
7 = unassigned
7 = unassigned
*/
void NavEKF3::getFilterTimeouts(int8_t instance, uint8_t &timeouts)
void NavEKF3::getFilterTimeouts(int8_t instance, uint8_t &timeouts) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1288,7 +1288,7 @@ void NavEKF3::getFilterTimeouts(int8_t instance, uint8_t &timeouts)
/*
return filter status flags
*/
void NavEKF3::getFilterStatus(int8_t instance, nav_filter_status &status)
void NavEKF3::getFilterStatus(int8_t instance, nav_filter_status &status) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1301,7 +1301,7 @@ void NavEKF3::getFilterStatus(int8_t instance, nav_filter_status &status)
/*
return filter gps quality check status
*/
void NavEKF3::getFilterGpsStatus(int8_t instance, nav_gps_status &status)
void NavEKF3::getFilterGpsStatus(int8_t instance, nav_gps_status &status) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1544,7 +1544,7 @@ void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
/*
get timing statistics structure
*/
void NavEKF3::getTimingStatistics(int8_t instance, struct ekf_timing &timing)
void NavEKF3::getTimingStatistics(int8_t instance, struct ekf_timing &timing) const
{
if (instance < 0 || instance >= num_cores) {
instance = primary;

46
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -72,38 +72,38 @@ public:
// An out of range instance (eg -1) returns data for the the primary instance
// If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control
bool getPosNE(int8_t instance, Vector2f &posNE);
bool getPosNE(int8_t instance, Vector2f &posNE) const;
// Write the last calculated D position relative to the reference point (m) for the specified instance.
// An out of range instance (eg -1) returns data for the the primary instance
// If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control
bool getPosD(int8_t instance, float &posD);
bool getPosD(int8_t instance, float &posD) const;
// return NED velocity in m/s for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getVelNED(int8_t instance, Vector3f &vel);
void getVelNED(int8_t instance, Vector3f &vel) const;
// Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
// This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF
// but will always be kinematically consistent with the z component of the EKF position state
float getPosDownDerivative(int8_t instance);
float getPosDownDerivative(int8_t instance) const;
// This returns the specific forces in the NED frame
void getAccelNED(Vector3f &accelNED) const;
// return body axis gyro bias estimates in rad/sec for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getGyroBias(int8_t instance, Vector3f &gyroBias);
void getGyroBias(int8_t instance, Vector3f &gyroBias) const;
// return accelerometer bias estimate in m/s/s
// An out of range instance (eg -1) returns data for the the primary instance
void getAccelBias(int8_t instance, Vector3f &accelBias);
void getAccelBias(int8_t instance, Vector3f &accelBias) const;
// return tilt error convergence metric for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getTiltError(int8_t instance, float &ang);
void getTiltError(int8_t instance, float &ang) const;
// reset body axis gyro bias estimates
void resetGyroBias(void);
@ -131,19 +131,19 @@ public:
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
// An out of range instance (eg -1) returns data for the the primary instance
void getWind(int8_t instance, Vector3f &wind);
void getWind(int8_t instance, Vector3f &wind) const;
// return earth magnetic field estimates in measurement units / 1000 for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getMagNED(int8_t instance, Vector3f &magNED);
void getMagNED(int8_t instance, Vector3f &magNED) const;
// return body magnetic field estimates in measurement units / 1000 for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getMagXYZ(int8_t instance, Vector3f &magXYZ);
void getMagXYZ(int8_t instance, Vector3f &magXYZ) const;
// return the magnetometer in use for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
uint8_t getActiveMag(int8_t instance);
uint8_t getActiveMag(int8_t instance) const;
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
@ -173,7 +173,7 @@ public:
// return the Euler roll, pitch and yaw angle in radians for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getEulerAngles(int8_t instance, Vector3f &eulers);
void getEulerAngles(int8_t instance, Vector3f &eulers) const;
// return the transformation matrix from XYZ (body) to NED axes
void getRotationBodyToNED(Matrix3f &mat) const;
@ -183,17 +183,17 @@ public:
// return the innovations for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov);
void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
// publish output observer angular, velocity and position tracking error
void getOutputTrackingError(int8_t instance, Vector3f &error) const;
// return the innovation consistency test ratios for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset);
void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
// return the diagonals from the covariance matrix for the specified instance
void getStateVariances(int8_t instance, float stateVar[24]);
void getStateVariances(int8_t instance, float stateVar[24]) const;
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()
@ -239,11 +239,11 @@ public:
*
* Return the system time stamp of the last update (msec)
*/
uint32_t getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar);
uint32_t getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar) const;
// return data for debugging optical flow fusion for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr);
void getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;
/*
Returns the following data for debugging range beacon fusion
@ -260,7 +260,7 @@ public:
returns true if data could be found, false if it could not
*/
bool getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED);
float &offsetHigh, float &offsetLow, Vector3f &posNED) const;
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
@ -287,7 +287,7 @@ public:
7 = badly conditioned synthetic sideslip fusion
7 = filter is not initialised
*/
void getFilterFaults(int8_t instance, uint16_t &faults);
void getFilterFaults(int8_t instance, uint16_t &faults) const;
/*
return filter timeout status as a bitmasked integer for the specified instance
@ -301,19 +301,19 @@ public:
7 = unassigned
7 = unassigned
*/
void getFilterTimeouts(int8_t instance, uint8_t &timeouts);
void getFilterTimeouts(int8_t instance, uint8_t &timeouts) const;
/*
return filter gps quality check status for the specified instance
An out of range instance (eg -1) returns data for the the primary instance
*/
void getFilterGpsStatus(int8_t instance, nav_gps_status &faults);
void getFilterGpsStatus(int8_t instance, nav_gps_status &faults) const;
/*
return filter status flags for the specified instance
An out of range instance (eg -1) returns data for the the primary instance
*/
void getFilterStatus(int8_t instance, nav_filter_status &status);
void getFilterStatus(int8_t instance, nav_filter_status &status) const;
// send an EKF_STATUS_REPORT message to GCS
void send_status_report(mavlink_channel_t chan);
@ -352,7 +352,7 @@ public:
bool have_ekf_logging(void) const { return logging.enabled && _logging_mask != 0; }
// get timing statistics structure
void getTimingStatistics(int8_t instance, struct ekf_timing &timing);
void getTimingStatistics(int8_t instance, struct ekf_timing &timing) const;
private:
uint8_t num_cores; // number of allocated cores