AP_NavEKF3: make logging a core concern

Also dissolve some methods only used for logging

Author: peterbarker

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -1239,39 +1239,6 @@ void NavEKF3::getMagXYZ(int8_t instance, Vector3f &magXYZ) const
     }
 }
 
-// return the magnetometer in use for the specified instance
-uint8_t NavEKF3::getActiveMag(int8_t instance) const
-{
-    if (instance < 0 || instance >= num_cores) instance = primary;
-    if (core) {
-        return core[instance].getActiveMag();
-    } else {
-        return 255;
-    }
-}
-
-// return the baro in use for the specified instance
-uint8_t NavEKF3::getActiveBaro(int8_t instance) const
-{
-    if (instance < 0 || instance >= num_cores) instance = primary;
-    if (core) {
-        return core[instance].getActiveBaro();
-    } else {
-        return 255;
-    }
-}
-
-// return the GPS in use for the specified instance
-uint8_t NavEKF3::getActiveGPS(int8_t instance) const
-{
-    if (instance < 0 || instance >= num_cores) instance = primary;
-    if (core) {
-        return core[instance].getActiveGPS();
-    } else {
-        return 255;
-    }
-}
-
 // return the airspeed sensor in use for the specified instance
 uint8_t NavEKF3::getActiveAirspeed(int8_t instance) const
 {
@@ -1408,15 +1375,6 @@ void NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posI
     }
 }
 
-// publish output observer angular, velocity and position tracking error
-void NavEKF3::getOutputTrackingError(int8_t instance, Vector3f &error) const
-{
-    if (instance < 0 || instance >= num_cores) instance = primary;
-    if (core) {
-        core[instance].getOutputTrackingError(error);
-    }
-}
-
 // 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) const
 {
@@ -1426,15 +1384,6 @@ 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]) const
-{
-    if (instance < 0 || instance >= num_cores) instance = primary;
-    if (core) {
-        core[instance].getStateVariances(stateVar);
-    }
-}
-
 // get a source's velocity innovations for the specified instance.  Set instance to -1 for the primary instance
 // returns true on success and results are placed in innovations and variances arguments
 bool NavEKF3::getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const
@@ -1538,15 +1487,6 @@ void NavEKF3::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSt
 }
 
 // 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) const
-{
-    if (instance < 0 || instance >= num_cores) instance = primary;
-    if (core) {
-        core[instance].getFlowDebug(varFlow, gndOffset, flowInnovX, flowInnovY, auxInnov, HAGL, rngInnov, range, gndOffsetErr);
-    }
-}
-
 /*
  * Write body frame linear and angular displacement measurements from a visual odometry sensor
  *
@@ -1603,19 +1543,6 @@ uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vec
     return ret;
 }
 
-// return data for debugging EKF-GSF yaw estimator
-// return false if data not available
-bool NavEKF3::getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const
-{
-    if (instance < 0 || instance >= num_cores) instance = primary;
-    if (core) {
-        if (core[instance].getDataEKFGSF(yaw_composite, yaw_composite_variance, yaw, innov_VN, innov_VE, weight)) {
-            return true;
-        }
-    }
-    return false;
-}
-
 // parameter conversion of EKF3 parameters
 void NavEKF3::convert_parameters()
 {
@@ -1717,18 +1644,6 @@ void NavEKF3::convert_parameters()
     }
 }
 
-// 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) const
-{
-    if (instance < 0 || instance >= num_cores) instance = primary;
-    if (core) {
-        return core[instance].getRangeBeaconDebug(ID, rng, innov, innovVar, testRatio, beaconPosNED, offsetHigh, offsetLow, posNED);
-    } else {
-        return false;
-    }
-}
-
 // called by vehicle code to specify that a takeoff is happening
 // causes the EKF to compensate for expected barometer errors due to rotor wash ground interaction
 // causes the EKF to start the EKF-GSF yaw estimator
@@ -2073,21 +1988,6 @@ void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
 
 }
 
-/*
-  get timing statistics structure
-*/
-void NavEKF3::getTimingStatistics(int8_t instance, struct ekf_timing &timing) const
-{
-    if (instance < 0 || instance >= num_cores) {
-        instance = primary;
-    }
-    if (core) {
-        core[instance].getTimingStatistics(timing);
-    } else {
-        memset(&timing, 0, sizeof(timing));
-    }
-}
-
 // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
 void NavEKF3::writeDefaultAirSpeed(float airspeed)
 {

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -141,9 +141,6 @@ class NavEKF3 {
 
     // return the sensor in use for the specified instance
     // An out of range instance (eg -1) returns data for the primary instance
-    uint8_t getActiveMag(int8_t instance) const;
-    uint8_t getActiveBaro(int8_t instance) const;
-    uint8_t getActiveGPS(int8_t instance) const;
     uint8_t getActiveAirspeed(int8_t instance) const;
 
     // Return estimated magnetometer offsets
@@ -189,16 +186,10 @@ class NavEKF3 {
     // An out of range instance (eg -1) returns data for the primary instance
     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 primary instance
     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]) const;
-
     // get a source's velocity innovations for the specified instance.  Set instance to -1 for the primary instance
     // returns true on success and results are placed in innovations and variances arguments
     bool getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
@@ -251,27 +242,6 @@ class NavEKF3 {
      */
     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 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) const;
-
-    /*
-        Returns the following data for debugging range beacon fusion
-        ID : beacon identifier
-        rng : measured range to beacon (m)
-        innov : range innovation (m)
-        innovVar : innovation variance (m^2)
-        testRatio : innovation consistency test ratio
-        beaconPosNED : beacon NED position (m)
-        offsetHigh : high hypothesis for range beacons system vertical offset (m)
-        offsetLow : low hypothesis for range beacons system vertical offset (m)
-        posNED : North,East,Down position estimate of receiver from 3-state filter
-
-        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) const;
-
     /*
      * Writes the measurement from a yaw angle sensor
      *
@@ -395,9 +365,6 @@ class NavEKF3 {
     // allow the enable flag to be set by Replay
     void set_enable(bool enable) { _enable.set_enable(enable); }
 
-    // get timing statistics structure
-    void getTimingStatistics(int8_t instance, struct ekf_timing &timing) const;
-
     /*
       check if switching lanes will reduce the normalised
       innovations. This is called when the vehicle code is about to
@@ -424,10 +391,6 @@ class NavEKF3 {
     // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
     void writeDefaultAirSpeed(float airspeed);
 
-    // log debug data for yaw estimator
-    // return false if data not available
-    bool getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const;
-
     // parameter conversion
     void convert_parameters();
 
@@ -611,20 +574,6 @@ class NavEKF3 {
     // checks for alignment
     bool coreBetterScore(uint8_t new_core, uint8_t current_core) const;
 
-    // logging functions shared by cores:
-    void Log_Write_XKF1(uint8_t core, uint64_t time_us) const;
-    void Log_Write_XKF2(uint8_t core, uint64_t time_us) const;
-    void Log_Write_XKF3(uint8_t core, uint64_t time_us) const;
-    void Log_Write_XKF4(uint8_t core, uint64_t time_us) const;
-    void Log_Write_XKF5(uint8_t core, uint64_t time_us) const;
-    void Log_Write_XKFS(uint8_t core, uint64_t time_us) const;
-    void Log_Write_Quaternion(uint8_t core, uint64_t time_us) const;
-    void Log_Write_Beacon(uint8_t core, uint64_t time_us) const;
-    void Log_Write_BodyOdom(uint8_t core, uint64_t time_us) const;
-    void Log_Write_State_Variances(uint8_t core, uint64_t time_us) const;
-    void Log_Write_Timing(uint8_t core, uint64_t time_us) const;
-    void Log_Write_GSF(uint8_t core, uint64_t time_us) const;
-
     // position, velocity and yaw source control
     AP_NavEKF_Source sources;
 };