Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: use mavlink messages not printf for state change notifications

Browse Source
This commit is contained in:
Andrew Tridgell 2016-07-19 12:16:25 +10:00
parent bd7bf21475
commit 567d5cdbe6
3 changed files with 13 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -8,6 +8,7 @@
#include "AP_NavEKF2_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
@ -221,7 +222,7 @@ void NavEKF2_core::setAidingMode()
// set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
if (PV_AidingMode == AID_NONE) {
// We have ceased aiding
hal.console->printf("EKF2 IMU%u has stopped aiding\n",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(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;
@ -238,7 +239,7 @@ void NavEKF2_core::setAidingMode()
stateStruct.position.z = -meaHgtAtTakeOff;
} else if (PV_AidingMode == AID_RELATIVE) {
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only
hal.console->printf("EKF2 IMU%u is using optical flow\n",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u is using optical flow",(unsigned)imu_index);
posTimeout = true;
velTimeout = true;
// Reset the last valid flow measurement time
@ -247,7 +248,7 @@ void NavEKF2_core::setAidingMode()
prevFlowFuseTime_ms = imuSampleTime_ms;
} else if (PV_AidingMode == AID_ABSOLUTE) {
// We have commenced aiding and GPS usage is allowed
hal.console->printf("EKF2 IMU%u is using GPS\n",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u is using GPS",(unsigned)imu_index);
posTimeout = false;
velTimeout = false;
// we need to reset the GPS timers to prevent GPS timeout logic being invoked on entry into GPS aiding
@ -276,7 +277,7 @@ void NavEKF2_core::checkAttitudeAlignmentStatus()
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
tiltAlignComplete = true;
hal.console->printf("EKF2 IMU%u tilt alignment complete\n",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u tilt alignment complete",(unsigned)imu_index);
}
// submit yaw and magnetic field reset requests depending on whether we have compass data
@ -352,7 +353,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 IMU%u Origin Set\n",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u Origin Set",(unsigned)imu_index);
}
// record a yaw reset event

9
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -8,6 +8,7 @@
#include "AP_NavEKF2_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
@ -112,14 +113,14 @@ void NavEKF2_core::controlMagYawReset()
// send initial alignment status to console
if (!yawAlignComplete) {
hal.console->printf("EKF2 IMU%u initial yaw alignment complete\n",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u initial yaw alignment complete",(unsigned)imu_index);
}
// send in-flight yaw alignment status to console
if (finalResetRequest) {
hal.console->printf("EKF2 IMU%u in-flight yaw alignment complete\n",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u in-flight yaw alignment complete",(unsigned)imu_index);
} else if (interimResetRequest) {
hal.console->printf("EKF2 IMU%u ground mag anomaly, yaw re-aligned\n",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index);
}
// update the yaw reset completed status
@ -169,7 +170,7 @@ void NavEKF2_core::realignYawGPS()
ResetPosition();
// send yaw alignment information to console
hal.console->printf("EKF2 IMU%u yaw aligned to GPS velocity\n",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index);
// zero the attitude covariances becasue the corelations will now be invalid
zeroAttCovOnly();

3
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -7,6 +7,7 @@
#include "AP_NavEKF2_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
@ -177,7 +178,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;
hal.console->printf("EKF2 IMU%u switching to compass %u\n",(unsigned)imu_index,magSelectIndex);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;