zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF2: const accessors

mission-4.1.18
Peter Barker 7 years ago committed by Francisco Ferreira
parent
aca87ab638
commit
81044760c7
2 changed files with 42 additions and 42 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 42
      libraries/AP_NavEKF2/AP_NavEKF2.cpp
  2. 42
      libraries/AP_NavEKF2/AP_NavEKF2.h

42
libraries/AP_NavEKF2/AP_NavEKF2.cpp
Unescape View File

@ -778,7 +778,7 @@ int8_t NavEKF2::getPrimaryCoreIMUIndex(void) const @@ -778,7 +778,7 @@ int8_t NavEKF2::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 NavEKF2::getPosNE(int8_t instance, Vector2f &posNE)
bool NavEKF2::getPosNE(int8_t instance, Vector2f &posNE) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (!core) {
@ -790,7 +790,7 @@ bool NavEKF2::getPosNE(int8_t instance, Vector2f &posNE) @@ -790,7 +790,7 @@ bool NavEKF2::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 NavEKF2::getPosD(int8_t instance, float &posD)
bool NavEKF2::getPosD(int8_t instance, float &posD) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (!core) {
@ -800,7 +800,7 @@ bool NavEKF2::getPosD(int8_t instance, float &posD) @@ -800,7 +800,7 @@ bool NavEKF2::getPosD(int8_t instance, float &posD)
}
// return NED velocity in m/s
void NavEKF2::getVelNED(int8_t instance, Vector3f &vel)
void NavEKF2::getVelNED(int8_t instance, Vector3f &vel) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -809,7 +809,7 @@ void NavEKF2::getVelNED(int8_t instance, Vector3f &vel) @@ -809,7 +809,7 @@ void NavEKF2::getVelNED(int8_t instance, Vector3f &vel)
}
// Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
float NavEKF2::getPosDownDerivative(int8_t instance)
float NavEKF2::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
@ -828,7 +828,7 @@ void NavEKF2::getAccelNED(Vector3f &accelNED) const @@ -828,7 +828,7 @@ void NavEKF2::getAccelNED(Vector3f &accelNED) const
}
// return body axis gyro bias estimates in rad/sec
void NavEKF2::getGyroBias(int8_t instance, Vector3f &gyroBias)
void NavEKF2::getGyroBias(int8_t instance, Vector3f &gyroBias) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -837,7 +837,7 @@ void NavEKF2::getGyroBias(int8_t instance, Vector3f &gyroBias) @@ -837,7 +837,7 @@ void NavEKF2::getGyroBias(int8_t instance, Vector3f &gyroBias)
}
// return body axis gyro scale factor error as a percentage
void NavEKF2::getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale)
void NavEKF2::getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -846,7 +846,7 @@ void NavEKF2::getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale) @@ -846,7 +846,7 @@ void NavEKF2::getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale)
}
// return tilt error convergence metric for the specified instance
void NavEKF2::getTiltError(int8_t instance, float &ang)
void NavEKF2::getTiltError(int8_t instance, float &ang) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -908,7 +908,7 @@ void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca @@ -908,7 +908,7 @@ void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca
}
// return the individual Z-accel bias estimates in m/s^2
void NavEKF2::getAccelZBias(int8_t instance, float &zbias)
void NavEKF2::getAccelZBias(int8_t instance, float &zbias) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -917,7 +917,7 @@ void NavEKF2::getAccelZBias(int8_t instance, float &zbias) @@ -917,7 +917,7 @@ void NavEKF2::getAccelZBias(int8_t instance, float &zbias)
}
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void NavEKF2::getWind(int8_t instance, Vector3f &wind)
void NavEKF2::getWind(int8_t instance, Vector3f &wind) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -926,7 +926,7 @@ void NavEKF2::getWind(int8_t instance, Vector3f &wind) @@ -926,7 +926,7 @@ void NavEKF2::getWind(int8_t instance, Vector3f &wind)
}
// return earth magnetic field estimates in measurement units / 1000
void NavEKF2::getMagNED(int8_t instance, Vector3f &magNED)
void NavEKF2::getMagNED(int8_t instance, Vector3f &magNED) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -935,7 +935,7 @@ void NavEKF2::getMagNED(int8_t instance, Vector3f &magNED) @@ -935,7 +935,7 @@ void NavEKF2::getMagNED(int8_t instance, Vector3f &magNED)
}
// return body magnetic field estimates in measurement units / 1000
void NavEKF2::getMagXYZ(int8_t instance, Vector3f &magXYZ)
void NavEKF2::getMagXYZ(int8_t instance, Vector3f &magXYZ) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -944,7 +944,7 @@ void NavEKF2::getMagXYZ(int8_t instance, Vector3f &magXYZ) @@ -944,7 +944,7 @@ void NavEKF2::getMagXYZ(int8_t instance, Vector3f &magXYZ)
}
// return the magnetometer in use for the specified instance
uint8_t NavEKF2::getActiveMag(int8_t instance)
uint8_t NavEKF2::getActiveMag(int8_t instance) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1047,7 +1047,7 @@ void NavEKF2::getQuaternion(int8_t instance, Quaternion &quat) const @@ -1047,7 +1047,7 @@ void NavEKF2::getQuaternion(int8_t instance, Quaternion &quat) const
}
// return the innovations for the specified instance
void NavEKF2::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov)
void NavEKF2::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) {
@ -1065,7 +1065,7 @@ void NavEKF2::getOutputTrackingError(int8_t instance, Vector3f &error) const @@ -1065,7 +1065,7 @@ void NavEKF2::getOutputTrackingError(int8_t instance, Vector3f &error) const
}
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void NavEKF2::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset)
void NavEKF2::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 NavEKF2::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, @@ -1101,7 +1101,7 @@ void NavEKF2::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates,
// return data for debugging optical flow fusion
void NavEKF2::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) {
@ -1110,7 +1110,7 @@ void NavEKF2::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, fl @@ -1110,7 +1110,7 @@ void NavEKF2::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, fl
}
// return data for debugging range beacon fusion
bool NavEKF2::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow)
bool NavEKF2::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1166,7 +1166,7 @@ void NavEKF2::setTerrainHgtStable(bool val) @@ -1166,7 +1166,7 @@ void NavEKF2::setTerrainHgtStable(bool val)
7 = badly conditioned synthetic sideslip fusion
7 = filter is not initialised
*/
void NavEKF2::getFilterFaults(int8_t instance, uint16_t &faults)
void NavEKF2::getFilterFaults(int8_t instance, uint16_t &faults) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1187,7 +1187,7 @@ void NavEKF2::getFilterFaults(int8_t instance, uint16_t &faults) @@ -1187,7 +1187,7 @@ void NavEKF2::getFilterFaults(int8_t instance, uint16_t &faults)
7 = unassigned
7 = unassigned
*/
void NavEKF2::getFilterTimeouts(int8_t instance, uint8_t &timeouts)
void NavEKF2::getFilterTimeouts(int8_t instance, uint8_t &timeouts) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1200,7 +1200,7 @@ void NavEKF2::getFilterTimeouts(int8_t instance, uint8_t &timeouts) @@ -1200,7 +1200,7 @@ void NavEKF2::getFilterTimeouts(int8_t instance, uint8_t &timeouts)
/*
return filter status flags
*/
void NavEKF2::getFilterStatus(int8_t instance, nav_filter_status &status)
void NavEKF2::getFilterStatus(int8_t instance, nav_filter_status &status) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1213,7 +1213,7 @@ void NavEKF2::getFilterStatus(int8_t instance, nav_filter_status &status) @@ -1213,7 +1213,7 @@ void NavEKF2::getFilterStatus(int8_t instance, nav_filter_status &status)
/*
return filter gps quality check status
*/
void NavEKF2::getFilterGpsStatus(int8_t instance, nav_gps_status &status)
void NavEKF2::getFilterGpsStatus(int8_t instance, nav_gps_status &status) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
@ -1456,7 +1456,7 @@ void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_ @@ -1456,7 +1456,7 @@ void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
/*
get timing statistics structure
*/
void NavEKF2::getTimingStatistics(int8_t instance, struct ekf_timing &timing)
void NavEKF2::getTimingStatistics(int8_t instance, struct ekf_timing &timing) const
{
if (instance < 0 || instance >= num_cores) {
instance = primary;

42
libraries/AP_NavEKF2/AP_NavEKF2.h
Unescape View File

@ -75,38 +75,38 @@ public: @@ -75,38 +75,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 diection (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 body axis gyro scale factor error as a percentage for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale);
void getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale) 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);
@ -136,23 +136,23 @@ public: @@ -136,23 +136,23 @@ public:
// return the Z-accel bias estimate in m/s^2 for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getAccelZBias(int8_t instance, float &zbias);
void getAccelZBias(int8_t instance, float &zbias) const;
// 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
@ -192,14 +192,14 @@ public: @@ -192,14 +192,14 @@ 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;
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()
@ -216,7 +216,7 @@ public: @@ -216,7 +216,7 @@ public:
// 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 from the specified instance
@ -229,7 +229,7 @@ public: @@ -229,7 +229,7 @@ public:
beaconPosNED : beacon NED position (m)
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);
bool getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow) 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
@ -256,7 +256,7 @@ public: @@ -256,7 +256,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
@ -270,19 +270,19 @@ public: @@ -270,19 +270,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);
@ -321,7 +321,7 @@ public: @@ -321,7 +321,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;
/*
* Write position and quaternion data from an external navigation system

Write Preview
Loading…
Cancel
Save