diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 46e91bd304..0f89eb3d06 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -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) // 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) } // 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) } // 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 } // 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) } // 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) } // 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 } // 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) } // 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) } // 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) } // 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) } // 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 } // 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 } // 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, // 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 } // 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) 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) 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) /* 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) /* 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_ /* 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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 87278547b6..9f6b2d1b5e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -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: // 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: // 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: // 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: 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: 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: 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: 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