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_NavEKF2: const accessors

Browse Source
This commit is contained in:
Peter Barker 2018-02-22 21:40:14 +11:00 committed by Francisco Ferreira
parent aca87ab638
commit 81044760c7
2 changed files with 42 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -778,7 +778,7 @@ int8_t NavEKF2::getPrimaryCoreIMUIndex(void) const
// Write the last calculated NE position relative to the reference point (m). // 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 a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (!core) { if (!core) {
@ -790,7 +790,7 @@ bool NavEKF2::getPosNE(int8_t instance, Vector2f &posNE)
// Write the last calculated D position relative to the reference point (m). // 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 a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (!core) { if (!core) {
@ -800,7 +800,7 @@ bool NavEKF2::getPosD(int8_t instance, float &posD)
} }
// return NED velocity in m/s // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -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 // 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; if (instance < 0 || instance >= num_cores) instance = primary;
// return the value calculated from a complementary filer applied to the EKF height and vertical acceleration // 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
} }
// return body axis gyro bias estimates in rad/sec // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -837,7 +837,7 @@ void NavEKF2::getGyroBias(int8_t instance, Vector3f &gyroBias)
} }
// return body axis gyro scale factor error as a percentage // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -846,7 +846,7 @@ void NavEKF2::getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale)
} }
// return tilt error convergence metric for the specified instance // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -908,7 +908,7 @@ void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca
} }
// return the individual Z-accel bias estimates in m/s^2 // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -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) // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -926,7 +926,7 @@ void NavEKF2::getWind(int8_t instance, Vector3f &wind)
} }
// return earth magnetic field estimates in measurement units / 1000 // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -935,7 +935,7 @@ void NavEKF2::getMagNED(int8_t instance, Vector3f &magNED)
} }
// return body magnetic field estimates in measurement units / 1000 // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -944,7 +944,7 @@ void NavEKF2::getMagXYZ(int8_t instance, Vector3f &magXYZ)
} }
// return the magnetometer in use for the specified instance // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -1047,7 +1047,7 @@ void NavEKF2::getQuaternion(int8_t instance, Quaternion &quat) const
} }
// return the innovations for the specified instance // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -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 // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -1101,7 +1101,7 @@ void NavEKF2::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates,
// return data for debugging optical flow fusion // return data for debugging optical flow fusion
void NavEKF2::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -1110,7 +1110,7 @@ void NavEKF2::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, fl
} }
// return data for debugging range beacon fusion // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -1166,7 +1166,7 @@ void NavEKF2::setTerrainHgtStable(bool val)
7 = badly conditioned synthetic sideslip fusion 7 = badly conditioned synthetic sideslip fusion
7 = filter is not initialised 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -1187,7 +1187,7 @@ void NavEKF2::getFilterFaults(int8_t instance, uint16_t &faults)
7 = unassigned 7 = unassigned
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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -1200,7 +1200,7 @@ void NavEKF2::getFilterTimeouts(int8_t instance, uint8_t &timeouts)
/* /*
return filter status flags 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -1213,7 +1213,7 @@ void NavEKF2::getFilterStatus(int8_t instance, nav_filter_status &status)
/* /*
return filter gps quality check 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
@ -1456,7 +1456,7 @@ void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
/* /*
get timing statistics structure 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) { if (instance < 0 || instance >= num_cores) {
instance = primary; instance = primary;

42
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -75,38 +75,38 @@ public:
// An out of range instance (eg -1) returns data for the the primary 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 a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control // 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. // 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 // 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 a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control // 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 // return NED velocity in m/s for the specified instance
// An out of range instance (eg -1) returns data for the the primary 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 // 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 // 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 // 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 // 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 // This returns the specific forces in the NED frame
void getAccelNED(Vector3f &accelNED) const; void getAccelNED(Vector3f &accelNED) const;
// return body axis gyro bias estimates in rad/sec for the specified instance // 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 // 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 // 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 // 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 // return tilt error convergence metric for the specified instance
// An out of range instance (eg -1) returns data for the the primary 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 // reset body axis gyro bias estimates
void resetGyroBias(void); void resetGyroBias(void);
@ -136,23 +136,23 @@ public:
// return the Z-accel bias estimate in m/s^2 for the specified instance // 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 // 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) // 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 // 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 // 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 // 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 // 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 // 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 // return the magnetometer in use for the specified instance
// An out of range instance (eg -1) returns data for the the primary 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 estimated magnetometer offsets
// Return true if magnetometer offsets are valid // Return true if magnetometer offsets are valid
@ -192,14 +192,14 @@ public:
// return the innovations for the specified instance // return the innovations for the specified instance
// An out of range instance (eg -1) returns data for the the primary 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 // publish output observer angular, velocity and position tracking error
void getOutputTrackingError(int8_t instance, Vector3f &error) const; void getOutputTrackingError(int8_t instance, Vector3f &error) const;
// return the innovation consistency test ratios for the specified instance // return the innovation consistency test ratios for the specified instance
// An out of range instance (eg -1) returns data for the the primary 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 // should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass() // reporting via ahrs.use_compass()
@ -216,7 +216,7 @@ public:
// return data for debugging optical flow fusion for the specified instance // 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 // 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 Returns the following data for debugging range beacon fusion from the specified instance
@ -229,7 +229,7 @@ public:
beaconPosNED : beacon NED position (m) beaconPosNED : beacon NED position (m)
returns true if data could be found, false if it could not 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 // called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect // causes the EKF to compensate for expected barometer errors due to ground effect
@ -256,7 +256,7 @@ public:
7 = badly conditioned synthetic sideslip fusion 7 = badly conditioned synthetic sideslip fusion
7 = filter is not initialised 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 return filter timeout status as a bitmasked integer for the specified instance
@ -270,19 +270,19 @@ public:
7 = unassigned 7 = unassigned
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 return filter gps quality check status for the specified instance
An out of range instance (eg -1) returns data for the the primary 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 return filter status flags for the specified instance
An out of range instance (eg -1) returns data for the the primary 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 // send an EKF_STATUS_REPORT message to GCS
void send_status_report(mavlink_channel_t chan); void send_status_report(mavlink_channel_t chan);
@ -321,7 +321,7 @@ public:
bool have_ekf_logging(void) const { return logging.enabled && _logging_mask != 0; } bool have_ekf_logging(void) const { return logging.enabled && _logging_mask != 0; }
// get timing statistics structure // 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 position and quaternion data from an external navigation system