diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index e42624b4b9..76e7cb4aae 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -539,28 +539,31 @@ int8_t NavEKF2::getPrimaryCoreIndex(void) const // Return the last calculated NED 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::getPosNED(Vector3f &pos) const +bool NavEKF2::getPosNED(int8_t instance, Vector3f &pos) { + if (instance < 0 || instance >= num_cores) instance = primary; if (!core) { return false; } - return core[primary].getPosNED(pos); + return core[instance].getPosNED(pos); } // return NED velocity in m/s -void NavEKF2::getVelNED(Vector3f &vel) const +void NavEKF2::getVelNED(int8_t instance, Vector3f &vel) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getVelNED(vel); + core[instance].getVelNED(vel); } } // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s -float NavEKF2::getPosDownDerivative(void) const +float NavEKF2::getPosDownDerivative(int8_t instance) { - // return the value calculated from a complmentary filer applied to the EKF height and vertical acceleration + if (instance < 0 || instance >= num_cores) instance = primary; + // return the value calculated from a complementary filer applied to the EKF height and vertical acceleration if (core) { - return core[primary].getPosDownDerivative(); + return core[instance].getPosDownDerivative(); } return 0.0f; } @@ -574,26 +577,29 @@ void NavEKF2::getAccelNED(Vector3f &accelNED) const } // return body axis gyro bias estimates in rad/sec -void NavEKF2::getGyroBias(Vector3f &gyroBias) const +void NavEKF2::getGyroBias(int8_t instance, Vector3f &gyroBias) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getGyroBias(gyroBias); + core[instance].getGyroBias(gyroBias); } } // return body axis gyro scale factor error as a percentage -void NavEKF2::getGyroScaleErrorPercentage(Vector3f &gyroScale) const +void NavEKF2::getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getGyroScaleErrorPercentage(gyroScale); + core[instance].getGyroScaleErrorPercentage(gyroScale); } } -// return tilt error convergence metric -void NavEKF2::getTiltError(float &ang) const +// return tilt error convergence metric for the specified instance +void NavEKF2::getTiltError(int8_t instance, float &ang) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getTiltError(ang); + core[instance].getTiltError(ang); } } @@ -642,34 +648,38 @@ void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca } // return the individual Z-accel bias estimates in m/s^2 -void NavEKF2::getAccelZBias(float &zbias) const +void NavEKF2::getAccelZBias(int8_t instance, float &zbias) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getAccelZBias(zbias); + core[instance].getAccelZBias(zbias); } } // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) -void NavEKF2::getWind(Vector3f &wind) const +void NavEKF2::getWind(int8_t instance, Vector3f &wind) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getWind(wind); + core[instance].getWind(wind); } } // return earth magnetic field estimates in measurement units / 1000 -void NavEKF2::getMagNED(Vector3f &magNED) const +void NavEKF2::getMagNED(int8_t instance, Vector3f &magNED) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getMagNED(magNED); + core[instance].getMagNED(magNED); } } // return body magnetic field estimates in measurement units / 1000 -void NavEKF2::getMagXYZ(Vector3f &magXYZ) const +void NavEKF2::getMagXYZ(int8_t instance, Vector3f &magXYZ) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getMagXYZ(magXYZ); + core[instance].getMagXYZ(magXYZ); } } @@ -728,11 +738,12 @@ bool NavEKF2::getHAGL(float &HAGL) const return core[primary].getHAGL(HAGL); } -// return the Euler roll, pitch and yaw angle in radians -void NavEKF2::getEulerAngles(Vector3f &eulers) const +// return the Euler roll, pitch and yaw angle in radians for the specified instance +void NavEKF2::getEulerAngles(int8_t instance, Vector3f &eulers) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getEulerAngles(eulers); + core[instance].getEulerAngles(eulers); } } @@ -752,19 +763,21 @@ void NavEKF2::getQuaternion(Quaternion &quat) const } } -// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements -void NavEKF2::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const +// return the innovations for the specified instance +void NavEKF2::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); + core[instance].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); } } // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements -void NavEKF2::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const +void NavEKF2::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); + core[instance].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); } } @@ -792,11 +805,12 @@ void NavEKF2::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, } // return data for debugging optical flow fusion -void NavEKF2::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, - float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const +void NavEKF2::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, + float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getFlowDebug(varFlow, gndOffset, flowInnovX, flowInnovY, auxInnov, HAGL, rngInnov, range, gndOffsetErr); + core[instance].getFlowDebug(varFlow, gndOffset, flowInnovX, flowInnovY, auxInnov, HAGL, rngInnov, range, gndOffsetErr); } } @@ -829,10 +843,11 @@ void NavEKF2::setTouchdownExpected(bool val) 7 = badly conditioned synthetic sideslip fusion 7 = filter is not initialised */ -void NavEKF2::getFilterFaults(uint8_t &faults) const +void NavEKF2::getFilterFaults(int8_t instance, uint8_t &faults) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getFilterFaults(faults); + core[instance].getFilterFaults(faults); } else { faults = 0; } @@ -849,10 +864,11 @@ void NavEKF2::getFilterFaults(uint8_t &faults) const 7 = unassigned 7 = unassigned */ -void NavEKF2::getFilterTimeouts(uint8_t &timeouts) const +void NavEKF2::getFilterTimeouts(int8_t instance, uint8_t &timeouts) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getFilterTimeouts(timeouts); + core[instance].getFilterTimeouts(timeouts); } else { timeouts = 0; } @@ -861,10 +877,11 @@ void NavEKF2::getFilterTimeouts(uint8_t &timeouts) const /* return filter status flags */ -void NavEKF2::getFilterStatus(nav_filter_status &status) const +void NavEKF2::getFilterStatus(int8_t instance, nav_filter_status &status) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getFilterStatus(status); + core[instance].getFilterStatus(status); } else { memset(&status, 0, sizeof(status)); } @@ -873,10 +890,11 @@ void NavEKF2::getFilterStatus(nav_filter_status &status) const /* return filter gps quality check status */ -void NavEKF2::getFilterGpsStatus(nav_gps_status &status) const +void NavEKF2::getFilterGpsStatus(int8_t instance, nav_gps_status &status) { + if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[primary].getFilterGpsStatus(status); + core[instance].getFilterGpsStatus(status); } else { memset(&status, 0, sizeof(status)); } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index c8a60bd7b7..19840a943d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -61,30 +61,36 @@ public: // return -1 if no primary core selected int8_t getPrimaryCoreIndex(void) const; - // Return the last calculated NED position relative to the reference point (m). + // Return the last calculated NED 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 getPosNED(Vector3f &pos) const; + bool getPosNED(int8_t instance, Vector3f &pos); - // return NED velocity in m/s - void getVelNED(Vector3f &vel) 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); - // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s + // 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(void) const; + float getPosDownDerivative(int8_t instance); // This returns the specific forces in the NED frame void getAccelNED(Vector3f &accelNED) const; - // return body axis gyro bias estimates in rad/sec - void getGyroBias(Vector3f &gyroBias) 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); - // return body axis gyro scale factor error as a percentage - void getGyroScaleErrorPercentage(Vector3f &gyroScale) 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); - // return tilt error convergence metric - void getTiltError(float &ang) 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); // reset body axis gyro bias estimates void resetGyroBias(void); @@ -108,17 +114,21 @@ public: // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const; - // return the Z-accel bias estimate in m/s^2 - void getAccelZBias(float &zbias) const; + // 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); // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) - void getWind(Vector3f &wind) const; + // An out of range instance (eg -1) returns data for the the primary instance + void getWind(int8_t instance, Vector3f &wind); - // return earth magnetic field estimates in measurement units / 1000 - void getMagNED(Vector3f &magNED) 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); - // return body magnetic field estimates in measurement units / 1000 - void getMagXYZ(Vector3f &magXYZ) 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); // Return estimated magnetometer offsets // Return true if magnetometer offsets are valid @@ -145,8 +155,9 @@ public: // return false if ground height is not being estimated. bool getHAGL(float &HAGL) const; - // return the Euler roll, pitch and yaw angle in radians - void getEulerAngles(Vector3f &eulers) const; + // 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); // return the transformation matrix from XYZ (body) to NED axes void getRotationBodyToNED(Matrix3f &mat) const; @@ -154,11 +165,13 @@ public: // return the quaternions defining the rotation from NED to XYZ (body) axes void getQuaternion(Quaternion &quat) const; - // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements - void getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; + // 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); - // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements - void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) 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); // should we use the compass? This is public so it can be used for // reporting via ahrs.use_compass() @@ -172,8 +185,9 @@ public: // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas); - // return data for debugging optical flow fusion - void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) 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); // called by vehicle code to specify that a takeoff is happening // causes the EKF to compensate for expected barometer errors due to ground effect @@ -184,7 +198,8 @@ public: void setTouchdownExpected(bool val); /* - return the filter fault status as a bitmasked integer + return the filter fault status as a bitmasked integer for the specified instance + An out of range instance (eg -1) returns data for the the primary instance 0 = quaternions are NaN 1 = velocities are NaN 2 = badly conditioned X magnetometer fusion @@ -194,10 +209,11 @@ public: 7 = badly conditioned synthetic sideslip fusion 7 = filter is not initialised */ - void getFilterFaults(uint8_t &faults) const; + void getFilterFaults(int8_t instance, uint8_t &faults); /* - return filter timeout status as a bitmasked integer + return filter timeout status as a bitmasked integer for the specified instance + An out of range instance (eg -1) returns data for the the primary instance 0 = position measurement timeout 1 = velocity measurement timeout 2 = height measurement timeout @@ -207,17 +223,19 @@ public: 7 = unassigned 7 = unassigned */ - void getFilterTimeouts(uint8_t &timeouts) const; + void getFilterTimeouts(int8_t instance, uint8_t &timeouts); /* - return filter gps quality check status + 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(nav_gps_status &faults) const; + void getFilterGpsStatus(int8_t instance, nav_gps_status &faults); /* - return filter status flags + return filter status flags for the specified instance + An out of range instance (eg -1) returns data for the the primary instance */ - void getFilterStatus(nav_filter_status &status) const; + void getFilterStatus(int8_t instance, nav_filter_status &status); // send an EKF_STATUS_REPORT message to GCS void send_status_report(mavlink_channel_t chan);