aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: eliminate GCS_MAVLINK::send_statustext_all

Browse Source
This commit is contained in:
Peter Barker 2017-07-09 14:16:08 +10:00 committed by Francisco Ferreira
parent 35d8f11e46
commit 5da3759ff4
5 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -656,7 +656,7 @@ bool NavEKF2::InitialiseFilter(void)
}
if (hal.util->available_memory() < sizeof(NavEKF2_core)*num_cores + 4096) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory");
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory");
_enable.set(0);
return false;
}
@ -664,7 +664,7 @@ bool NavEKF2::InitialiseFilter(void)
core = new NavEKF2_core[num_cores];
if (core == nullptr) {
_enable.set(0);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF2: allocation failed");
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: allocation failed");
return false;
}

16
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -269,7 +269,7 @@ void NavEKF2_core::setAidingMode()
switch (PV_AidingMode) {
case AID_NONE:
// We have ceased aiding
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index);
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
posTimeout = true;
velTimeout = true;
@ -288,7 +288,7 @@ void NavEKF2_core::setAidingMode()
case AID_RELATIVE:
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index);
posTimeout = true;
velTimeout = true;
// Reset the last valid flow measurement time
@ -302,15 +302,15 @@ void NavEKF2_core::setAidingMode()
bool canUseRangeBeacon = readyToUseRangeBeacon();
// We have commenced aiding and GPS usage is allowed
if (canUseGPS) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index);
}
posTimeout = false;
velTimeout = false;
// We have commenced aiding and range beacon usage is allowed
if (canUseRangeBeacon) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using range beacons",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using range beacons",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset);
}
// reset the last fusion accepted times to prevent unwanted activation of timeout logic
lastPosPassTime_ms = imuSampleTime_ms;
@ -339,7 +339,7 @@ void NavEKF2_core::checkAttitudeAlignmentStatus()
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
tiltAlignComplete = true;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u tilt alignment complete",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u tilt alignment complete",(unsigned)imu_index);
}
// submit yaw and magnetic field reset requests depending on whether we have compass data
@ -425,7 +425,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;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u Origin set to GPS",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u Origin set to GPS",(unsigned)imu_index);
}
// record a yaw reset event

8
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -111,14 +111,14 @@ void NavEKF2_core::controlMagYawReset()
// send initial alignment status to console
if (!yawAlignComplete) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u initial yaw alignment complete",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial yaw alignment complete",(unsigned)imu_index);
}
// send in-flight yaw alignment status to console
if (finalResetRequest) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u in-flight yaw alignment complete",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u in-flight yaw alignment complete",(unsigned)imu_index);
} else if (interimResetRequest) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index);
}
// update the yaw reset completed status
@ -168,7 +168,7 @@ void NavEKF2_core::realignYawGPS()
ResetPosition();
// send yaw alignment information to console
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index);
// zero the attitude covariances becasue the corelations will now be invalid
zeroAttCovOnly();

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -215,7 +215,7 @@ void NavEKF2_core::readMagData()
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;

2
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -86,7 +86,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
// capable of giving a vertical velocity
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
frontend->_fusionModeGPS.set(1);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1");
gcs().send_text(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1");
}
} else {
gpsVertVelFail = false;