From 49f9ea317c448d2d6cde5db008a90bfef33a884f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 6 Nov 2015 12:06:25 +1100 Subject: [PATCH] AP_NavEKF: Update console message and param descriptions for multiple EKF's --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 3 ++- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 10 +++++----- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 2 -- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 60cc07f49a..79b30ab9b6 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -397,7 +397,8 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @Param: IMU_MASK // @DisplayName: Bitmask of active IMUs - // @Description: 1 byte bitmap of IMUs to use in EKF2 + // @Description: 1 byte bitmap of IMUs to use in EKF2. A separate instance of EKF2 will be started for each IMU selected. Set to 1 to use the first IMU only (default), set to 2 to use the second IMU only, set to 3 to use the first and second IMU. Additional IMU's can be used up to a maximum of 6 if memory and processing resources permit. There may be insufficient memory and processing resources to run multiple instances. If this occurs EKF2 will fail to start. + // @Range: 1 127 // @User: Advanced AP_GROUPINFO("IMU_MASK", 33, NavEKF2, _imuMask, 1), diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index d71807f382..ac0eaadcba 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -123,7 +123,7 @@ void NavEKF2_core::setAidingMode() stateStruct.position.z = -meaHgtAtTakeOff; } else if (frontend->_fusionModeGPS == 3) { // We have commenced aiding, but GPS useage has been prohibited so use optical flow only - hal.console->printf("EKF2 is using optical flow\n"); + hal.console->printf("EKF2 IMU%i is using optical flow\n",imu_index); PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states posTimeout = true; velTimeout = true; @@ -133,7 +133,7 @@ void NavEKF2_core::setAidingMode() prevFlowFuseTime_ms = imuSampleTime_ms; } else { // We have commenced aiding and GPS useage is allowed - hal.console->printf("EKF2 is using GPS\n"); + hal.console->printf("EKF2 IMU%i is using GPS\n",imu_index); PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states posTimeout = false; velTimeout = false; @@ -169,7 +169,7 @@ void NavEKF2_core::checkAttitudeAlignmentStatus() tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt; if (tiltErrFilt < 0.005f && !tiltAlignComplete) { tiltAlignComplete = true; - hal.console->printf("EKF2 tilt alignment complete\n"); + hal.console->printf("EKF2 IMU%i tilt alignment complete\n",imu_index); } // Once tilt has converged, align yaw using magnetic field measurements @@ -179,7 +179,7 @@ void NavEKF2_core::checkAttitudeAlignmentStatus() stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); StoreQuatReset(); yawAlignComplete = true; - hal.console->printf("EKF2 yaw alignment complete\n"); + hal.console->printf("EKF2 IMU%i yaw alignment complete\n",imu_index); } } @@ -244,7 +244,7 @@ void NavEKF2_core::setOrigin() // define Earth rotation vector in the NED navigation frame at the origin calcEarthRateNED(earthRateNED, _ahrs->get_home().lat); validOrigin = true; - hal.console->printf("EKF2 Origin Set\n"); + hal.console->printf("EKF2 IMU%i Origin Set\n",imu_index); } // Commands the EKF to not use GPS. diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index d2c822a75a..71e52fd43b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -283,8 +283,6 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) // set to true now that states have be initialised statesInitialised = true; - hal.console->printf("EKF2 initialised on IMU %u\n", (unsigned)imu_index); - return true; }