/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include #if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 /* optionally turn down optimisation for debugging */ // #pragma GCC optimize("O0") #include "AP_NavEKF2.h" #include "AP_NavEKF2_core.h" #include #include #include extern const AP_HAL::HAL& hal; // Check basic filter health metrics and return a consolidated health status bool NavEKF2_core::healthy(void) const { uint8_t faultInt; getFilterFaults(faultInt); if (faultInt > 0) { return false; } if (velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) { // all three metrics being above 1 means the filter is // extremely unhealthy. return false; } // Give the filter a second to settle before use if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) { return false; } // barometer and position innovations must be within limits when on-ground float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]); if (onGround && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) { return false; } // all OK return true; } // return data for debugging optical flow fusion void NavEKF2_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const { varFlow = max(flowTestRatio[0],flowTestRatio[1]); gndOffset = terrainState; flowInnovX = innovOptFlow[0]; flowInnovY = innovOptFlow[1]; auxInnov = auxFlowObsInnov; HAGL = terrainState - stateStruct.position.z; rngInnov = innovRng; range = rngMea; gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset() } // provides the height limit to be observed by the control loops // returns false if no height limiting is required // this is needed to ensure the vehicle does not fly too high when using optical flow navigation bool NavEKF2_core::getHeightControlLimit(float &height) const { // only ask for limiting if we are doing optical flow navigation if (frontend._fusionModeGPS == 3) { // If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors height = max(float(_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f); return true; } else { return false; } } // return the Euler roll, pitch and yaw angle in radians void NavEKF2_core::getEulerAngles(Vector3f &euler) const { outputDataNew.quat.to_euler(euler.x, euler.y, euler.z); euler = euler - _ahrs->get_trim(); } // return body axis gyro bias estimates in rad/sec void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const { if (dtIMUavg < 1e-6f) { gyroBias.zero(); return; } gyroBias = stateStruct.gyro_bias / dtIMUavg; } // return body axis gyro scale factor error as a percentage void NavEKF2_core::getGyroScaleErrorPercentage(Vector3f &gyroScale) const { if (!statesInitialised) { gyroScale.x = gyroScale.y = gyroScale.z = 0; return; } gyroScale.x = 100.0f/stateStruct.gyro_scale.x - 100.0f; gyroScale.y = 100.0f/stateStruct.gyro_scale.y - 100.0f; gyroScale.z = 100.0f/stateStruct.gyro_scale.z - 100.0f; } // return tilt error convergence metric void NavEKF2_core::getTiltError(float &ang) const { ang = tiltErrFilt; } // return the transformation matrix from XYZ (body) to NED axes void NavEKF2_core::getRotationBodyToNED(Matrix3f &mat) const { Vector3f trim = _ahrs->get_trim(); outputDataNew.quat.rotation_matrix(mat); mat.rotateXYinv(trim); } // return the quaternions defining the rotation from NED to XYZ (body) axes void NavEKF2_core::getQuaternion(Quaternion& ret) const { ret = outputDataNew.quat; } // return the amount of yaw angle change due to the last yaw angle reset in radians // returns the time of the last yaw angle reset or 0 if no reset has ever occurred uint32_t NavEKF2_core::getLastYawResetAngle(float &yawAng) { yawAng = yawResetAngle; return lastYawReset_ms; } // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) void NavEKF2_core::getWind(Vector3f &wind) const { wind.x = stateStruct.wind_vel.x; wind.y = stateStruct.wind_vel.y; wind.z = 0.0f; // currently don't estimate this } // return NED velocity in m/s // void NavEKF2_core::getVelNED(Vector3f &vel) const { vel = outputDataNew.velocity; } // This returns the specific forces in the NED frame void NavEKF2_core::getAccelNED(Vector3f &accelNED) const { accelNED = velDotNED; accelNED.z -= GRAVITY_MSS; } // return the Z-accel bias estimate in m/s^2 void NavEKF2_core::getAccelZBias(float &zbias) const { if (dtIMUavg > 0) { zbias = stateStruct.accel_zbias / dtIMUavg; } else { zbias = 0; } } // 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 bool NavEKF2_core::getPosNED(Vector3f &pos) const { // The EKF always has a height estimate regardless of mode of operation pos.z = outputDataNew.position.z; // There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available) nav_filter_status status; getFilterStatus(status); if (status.flags.horiz_pos_abs || status.flags.horiz_pos_rel) { // This is the normal mode of operation where we can use the EKF position states pos.x = outputDataNew.position.x; pos.y = outputDataNew.position.y; return true; } else { // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate if(validOrigin) { if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { // If the origin has been set and we have GPS, then return the GPS position relative to the origin const struct Location &gpsloc = _ahrs->get_gps().location(); Vector2f tempPosNE = location_diff(EKF_origin, gpsloc); pos.x = tempPosNE.x; pos.y = tempPosNE.y; return false; } else { // If no GPS fix is available, all we can do is provide the last known position pos.x = outputDataNew.position.x; pos.y = outputDataNew.position.y; return false; } } else { // If the origin has not been set, then we have no means of providing a relative position pos.x = 0.0f; pos.y = 0.0f; return false; } } return false; } // return the estimated height above ground level bool NavEKF2_core::getHAGL(float &HAGL) const { HAGL = terrainState - outputDataNew.position.z; // If we know the terrain offset and altitude, then we have a valid height above ground estimate return !hgtTimeout && gndOffsetValid && healthy(); } // Return the last calculated latitude, longitude and height in WGS-84 // If a calculated location isn't available, return a raw GPS measurement // The status will return true if a calculation or raw measurement is available // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control bool NavEKF2_core::getLLH(struct Location &loc) const { if(validOrigin) { // Altitude returned is an absolute altitude relative to the WGS-84 spherioid loc.alt = EKF_origin.alt - outputDataNew.position.z*100; loc.flags.relative_alt = 0; loc.flags.terrain_alt = 0; // there are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no aiding) nav_filter_status status; getFilterStatus(status); if (status.flags.horiz_pos_abs || status.flags.horiz_pos_rel) { loc.lat = EKF_origin.lat; loc.lng = EKF_origin.lng; location_offset(loc, outputDataNew.position.x, outputDataNew.position.y); return true; } else { // we could be in constant position mode becasue the vehicle has taken off without GPS, or has lost GPS // in this mode we cannot use the EKF states to estimate position so will return the best available data if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { // we have a GPS position fix to return const struct Location &gpsloc = _ahrs->get_gps().location(); loc.lat = gpsloc.lat; loc.lng = gpsloc.lng; return true; } else { // if no GPS fix, provide last known position before entering the mode location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y); return false; } } } else { // If no origin has been defined for the EKF, then we cannot use its position states so return a raw // GPS reading if available and return false if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D)) { const struct Location &gpsloc = _ahrs->get_gps().location(); loc = gpsloc; loc.flags.relative_alt = 0; loc.flags.terrain_alt = 0; } return false; } } // return the horizontal speed limit in m/s set by optical flow sensor limits // 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 NavEKF2_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const { if (PV_AidingMode == AID_RELATIVE) { // allow 1.0 rad/sec margin for angular motion ekfGndSpdLimit = max((frontend._maxFlowRate - 1.0f), 0.0f) * max((terrainState - stateStruct.position[2]), rngOnGnd); // use standard gains up to 5.0 metres height and reduce above that ekfNavVelGainScaler = 4.0f / max((terrainState - stateStruct.position[2]),4.0f); } else { ekfGndSpdLimit = 400.0f; //return 80% of max filter speed ekfNavVelGainScaler = 1.0f; } } // return the LLH location of the filters NED origin bool NavEKF2_core::getOriginLLH(struct Location &loc) const { if (validOrigin) { loc = EKF_origin; } return validOrigin; } // return earth magnetic field estimates in measurement units / 1000 void NavEKF2_core::getMagNED(Vector3f &magNED) const { magNED = stateStruct.earth_magfield * 1000.0f; } // return body magnetic field estimates in measurement units / 1000 void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const { magXYZ = stateStruct.body_magfield*1000.0f; } // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements void NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const { velInnov.x = innovVelPos[0]; velInnov.y = innovVelPos[1]; velInnov.z = innovVelPos[2]; posInnov.x = innovVelPos[3]; posInnov.y = innovVelPos[4]; posInnov.z = innovVelPos[5]; magInnov.x = 1e3f*innovMag[0]; // Convert back to sensor units magInnov.y = 1e3f*innovMag[1]; // Convert back to sensor units magInnov.z = 1e3f*innovMag[2]; // Convert back to sensor units tasInnov = innovVtas; yawInnov = innovYaw; } // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements // this indicates the amount of margin available when tuning the various error traps // also return the current offsets applied to the GPS position measurements void NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const { velVar = sqrtf(velTestRatio); posVar = sqrtf(posTestRatio); hgtVar = sqrtf(hgtTestRatio); magVar.x = sqrtf(magTestRatio.x); magVar.y = sqrtf(magTestRatio.y); magVar.z = sqrtf(magTestRatio.z); tasVar = sqrtf(tasTestRatio); offset = gpsPosGlitchOffsetNE; } /* return the filter fault status as a bitmasked integer 0 = quaternions are NaN 1 = velocities are NaN 2 = badly conditioned X magnetometer fusion 3 = badly conditioned Y magnetometer fusion 5 = badly conditioned Z magnetometer fusion 6 = badly conditioned airspeed fusion 7 = badly conditioned synthetic sideslip fusion 7 = filter is not initialised */ void NavEKF2_core::getFilterFaults(uint8_t &faults) const { faults = (stateStruct.quat.is_nan()<<0 | stateStruct.velocity.is_nan()<<1 | faultStatus.bad_xmag<<2 | faultStatus.bad_ymag<<3 | faultStatus.bad_zmag<<4 | faultStatus.bad_airspeed<<5 | faultStatus.bad_sideslip<<6 | !statesInitialised<<7); } /* return filter timeout status as a bitmasked integer 0 = position measurement timeout 1 = velocity measurement timeout 2 = height measurement timeout 3 = magnetometer measurement timeout 4 = true airspeed measurement timeout 5 = unassigned 6 = unassigned 7 = unassigned */ void NavEKF2_core::getFilterTimeouts(uint8_t &timeouts) const { timeouts = (posTimeout<<0 | velTimeout<<1 | hgtTimeout<<2 | magTimeout<<3 | tasTimeout<<4); } /* return filter function status as a bitmasked integer 0 = attitude estimate valid 1 = horizontal velocity estimate valid 2 = vertical velocity estimate valid 3 = relative horizontal position estimate valid 4 = absolute horizontal position estimate valid 5 = vertical position estimate valid 6 = terrain height estimate valid 7 = constant position mode */ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const { // init return value status.value = 0; bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid; bool doingWindRelNav = !tasTimeout && assume_zero_sideslip(); bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE); bool notDeadReckoning = (PV_AidingMode == AID_ABSOLUTE); bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout; bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; bool optFlowNavPossible = flowDataValid && (frontend._fusionModeGPS == 3); bool gpsNavPossible = !gpsNotAvailable && (frontend._fusionModeGPS <= 2); bool filterHealthy = healthy() && tiltAlignComplete && yawAlignComplete; // set individual flags status.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) status.flags.horiz_vel = someHorizRefData && notDeadReckoning && filterHealthy; // horizontal velocity estimate valid status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning && filterHealthy; // relative horizontal position estimate valid status.flags.horiz_pos_abs = doingNormalGpsNav && notDeadReckoning && filterHealthy; // absolute horizontal position estimate valid status.flags.vert_pos = !hgtTimeout && filterHealthy; // vertical position estimate valid status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid status.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode status.flags.pred_horiz_pos_rel = (optFlowNavPossible || gpsNavPossible) && filterHealthy; // we should be able to estimate a relative position when we enter flight mode status.flags.pred_horiz_pos_abs = gpsNavPossible && filterHealthy; // we should be able to estimate an absolute position when we enter flight mode status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected status.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode status.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode status.flags.using_gps = (imuSampleTime_ms - lastPosPassTime_ms) < 4000; } // send an EKF_STATUS message to GCS void NavEKF2_core::send_status_report(mavlink_channel_t chan) { // get filter status nav_filter_status filt_state; getFilterStatus(filt_state); // prepare flags uint16_t flags = 0; if (filt_state.flags.attitude) { flags |= EKF_ATTITUDE; } if (filt_state.flags.horiz_vel) { flags |= EKF_VELOCITY_HORIZ; } if (filt_state.flags.vert_vel) { flags |= EKF_VELOCITY_VERT; } if (filt_state.flags.horiz_pos_rel) { flags |= EKF_POS_HORIZ_REL; } if (filt_state.flags.horiz_pos_abs) { flags |= EKF_POS_HORIZ_ABS; } if (filt_state.flags.vert_pos) { flags |= EKF_POS_VERT_ABS; } if (filt_state.flags.terrain_alt) { flags |= EKF_POS_VERT_AGL; } if (filt_state.flags.const_pos_mode) { flags |= EKF_CONST_POS_MODE; } if (filt_state.flags.pred_horiz_pos_rel) { flags |= EKF_PRED_POS_HORIZ_REL; } if (filt_state.flags.pred_horiz_pos_abs) { flags |= EKF_PRED_POS_HORIZ_ABS; } // get variances float velVar, posVar, hgtVar, tasVar; Vector3f magVar; Vector2f offset; getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); // send message mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), tasVar); } #endif // HAL_CPU_CLASS