fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 14:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink_log_info: always print to console and merge with mavlink_and_console_log_info

Browse Source
This commit is contained in:
Daniel Agar
2020-05-12 08:53:51 +02:00
committed by Beat Küng
parent b933557f56
commit 05886e053f
19 changed files with 125 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+11 -11
View File
@@ -207,13 +207,13 @@ void BlockLocalPositionEstimator::Run()
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_lidar == nullptr) {
_sub_lidar = s;
mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %zu", msg_label, i);
mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %zu", msg_label, i);
} else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_sonar == nullptr) {
_sub_sonar = s;
mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %zu", msg_label, i);
mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %zu", msg_label, i);
}
}
}
@@ -346,8 +346,8 @@ void BlockLocalPositionEstimator::Run()
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
double(_param_lpe_lat.get()), double(_param_lpe_lon.get()), double(_altOrigin));
mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
double(_param_lpe_lat.get()), double(_param_lpe_lon.get()), double(_altOrigin));
}
// reinitialize x if necessary
@@ -359,7 +359,7 @@ void BlockLocalPositionEstimator::Run()
// don't want it to take too long
if (!PX4_ISFINITE(_x(i))) {
reinit_x = true;
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%zu) not finite", msg_label, i);
mavlink_log_info(&mavlink_log_pub, "%sreinit x, x(%zu) not finite", msg_label, i);
break;
}
}
@@ -369,7 +369,7 @@ void BlockLocalPositionEstimator::Run()
_x(i) = 0;
}
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
mavlink_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
}
// force P symmetry and reinitialize P if necessary
@@ -378,16 +378,16 @@ void BlockLocalPositionEstimator::Run()
for (size_t i = 0; i < n_x; i++) {
for (size_t j = 0; j <= i; j++) {
if (!PX4_ISFINITE(m_P(i, j))) {
mavlink_and_console_log_info(&mavlink_log_pub,
"%sreinit P (%zu, %zu) not finite", msg_label, i, j);
mavlink_log_info(&mavlink_log_pub,
"%sreinit P (%zu, %zu) not finite", msg_label, i, j);
reinit_P = true;
}
if (i == j) {
// make sure diagonal elements are positive
if (m_P(i, i) <= 0) {
mavlink_and_console_log_info(&mavlink_log_pub,
"%sreinit P (%zu, %zu) negative", msg_label, i, j);
mavlink_log_info(&mavlink_log_pub,
"%sreinit P (%zu, %zu) negative", msg_label, i, j);
reinit_P = true;
}
@@ -1006,7 +1006,7 @@ int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
*periods = i_hist;
if (t_delay > DELAY_MAX) {
mavlink_and_console_log_info(&mavlink_log_pub, "%sdelayed data old: %8.4f", msg_label, double(t_delay));
mavlink_log_info(&mavlink_log_pub, "%sdelayed data old: %8.4f", msg_label, double(t_delay));
return -1;
}
src/modules/local_position_estimator/sensors/baro.cpp
+6 -6
View File
@@ -22,10 +22,10 @@ void BlockLocalPositionEstimator::baroInit()
// if finished
if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
_baroAltOrigin = _baroStats.getMean()(0);
mavlink_and_console_log_info(&mavlink_log_pub,
"[lpe] baro init %d m std %d cm",
(int)_baroStats.getMean()(0),
(int)(100 * _baroStats.getStdDev()(0)));
mavlink_log_info(&mavlink_log_pub,
"[lpe] baro init %d m std %d cm",
(int)_baroStats.getMean()(0),
(int)(100 * _baroStats.getStdDev()(0)));
_sensorTimeout &= ~SENSOR_BARO;
_sensorFault &= ~SENSOR_BARO;
@@ -83,7 +83,7 @@ void BlockLocalPositionEstimator::baroCorrect()
} else if (_sensorFault & SENSOR_BARO) {
_sensorFault &= ~SENSOR_BARO;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK");
mavlink_log_info(&mavlink_log_pub, "[lpe] baro OK");
}
// kalman filter correction always
@@ -99,7 +99,7 @@ void BlockLocalPositionEstimator::baroCheckTimeout()
if (!(_sensorTimeout & SENSOR_BARO)) {
_sensorTimeout |= SENSOR_BARO;
_baroStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro timeout ");
mavlink_log_info(&mavlink_log_pub, "[lpe] baro timeout ");
}
}
}
src/modules/local_position_estimator/sensors/flow.cpp
+6 -6
View File
@@ -22,10 +22,10 @@ void BlockLocalPositionEstimator::flowInit()
// if finished
if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow init: "
"quality %d std %d",
int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0)));
mavlink_log_info(&mavlink_log_pub, "[lpe] flow init: "
"quality %d std %d",
int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0)));
_sensorTimeout &= ~SENSOR_FLOW;
_sensorFault &= ~SENSOR_FLOW;
}
@@ -185,13 +185,13 @@ void BlockLocalPositionEstimator::flowCorrect()
if (beta > BETA_TABLE[n_y_flow]) {
if (!(_sensorFault & SENSOR_FLOW)) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
mavlink_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_FLOW;
}
} else if (_sensorFault & SENSOR_FLOW) {
_sensorFault &= ~SENSOR_FLOW;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
mavlink_log_info(&mavlink_log_pub, "[lpe] flow OK");
}
if (!(_sensorFault & SENSOR_FLOW)) {
src/modules/local_position_estimator/sensors/gps.cpp
+3 -3
View File
@@ -75,8 +75,8 @@ void BlockLocalPositionEstimator::gpsInit()
_altOriginInitialized = true;
_altOriginGlobal = true;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (gps) : lat %6.2f lon %6.2f alt %5.1f m",
gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin));
mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (gps) : lat %6.2f lon %6.2f alt %5.1f m",
gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin));
}
PX4_INFO("[lpe] gps init "
@@ -223,7 +223,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
} else if (_sensorFault & SENSOR_GPS) {
_sensorFault &= ~SENSOR_GPS;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
mavlink_log_info(&mavlink_log_pub, "[lpe] GPS OK");
}
// kalman filter correction always for GPS
src/modules/local_position_estimator/sensors/land.cpp
+4 -4
View File
@@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::landInit()
// if finished
if (_landCount > REQ_LAND_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land init");
mavlink_log_info(&mavlink_log_pub, "[lpe] land init");
_sensorTimeout &= ~SENSOR_LAND;
_sensorFault &= ~SENSOR_LAND;
}
@@ -73,7 +73,7 @@ void BlockLocalPositionEstimator::landCorrect()
if (beta / BETA_TABLE[n_y_land] > beta_thresh) {
if (!(_sensorFault & SENSOR_LAND)) {
_sensorFault |= SENSOR_LAND;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land fault, beta %5.2f", double(beta));
mavlink_log_info(&mavlink_log_pub, "[lpe] land fault, beta %5.2f", double(beta));
}
// abort correction
@@ -81,7 +81,7 @@ void BlockLocalPositionEstimator::landCorrect()
} else if (_sensorFault & SENSOR_LAND) {
_sensorFault &= ~SENSOR_LAND;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK");
mavlink_log_info(&mavlink_log_pub, "[lpe] land OK");
}
// kalman filter correction always for land detector
@@ -97,7 +97,7 @@ void BlockLocalPositionEstimator::landCheckTimeout()
if (!(_sensorTimeout & SENSOR_LAND)) {
_sensorTimeout |= SENSOR_LAND;
_landCount = 0;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land timeout ");
mavlink_log_info(&mavlink_log_pub, "[lpe] land timeout ");
}
}
}
src/modules/local_position_estimator/sensors/landing_target.cpp
+4 -4
View File
@@ -16,7 +16,7 @@ void BlockLocalPositionEstimator::landingTargetInit()
Vector<float, n_y_target> y;
if (landingTargetMeasure(y) == OK) {
mavlink_and_console_log_info(&mavlink_log_pub, "Landing target init");
mavlink_log_info(&mavlink_log_pub, "Landing target init");
_sensorTimeout &= ~SENSOR_LAND_TARGET;
_sensorFault &= ~SENSOR_LAND_TARGET;
}
@@ -89,7 +89,7 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
if (beta > BETA_TABLE[n_y_target]) {
if (!(_sensorFault & SENSOR_LAND_TARGET)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Landing target fault, beta %5.2f", double(beta));
mavlink_log_info(&mavlink_log_pub, "Landing target fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_LAND_TARGET;
}
@@ -98,7 +98,7 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
} else if (_sensorFault & SENSOR_LAND_TARGET) {
_sensorFault &= ~SENSOR_LAND_TARGET;
mavlink_and_console_log_info(&mavlink_log_pub, "Landing target OK");
mavlink_log_info(&mavlink_log_pub, "Landing target OK");
}
// kalman filter correction
@@ -115,7 +115,7 @@ void BlockLocalPositionEstimator::landingTargetCheckTimeout()
if (_timeStamp - _time_last_target > TARGET_TIMEOUT) {
if (!(_sensorTimeout & SENSOR_LAND_TARGET)) {
_sensorTimeout |= SENSOR_LAND_TARGET;
mavlink_and_console_log_info(&mavlink_log_pub, "Landing target timeout");
mavlink_log_info(&mavlink_log_pub, "Landing target timeout");
}
}
}
src/modules/local_position_estimator/sensors/lidar.cpp
+7 -7
View File
@@ -21,10 +21,10 @@ void BlockLocalPositionEstimator::lidarInit()
// if finished
if (_lidarStats.getCount() > REQ_LIDAR_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar init: "
"mean %d cm stddev %d cm",
int(100 * _lidarStats.getMean()(0)),
int(100 * _lidarStats.getStdDev()(0)));
mavlink_log_info(&mavlink_log_pub, "[lpe] lidar init: "
"mean %d cm stddev %d cm",
int(100 * _lidarStats.getMean()(0)),
int(100 * _lidarStats.getStdDev()(0)));
_sensorTimeout &= ~SENSOR_LIDAR;
_sensorFault &= ~SENSOR_LIDAR;
}
@@ -103,7 +103,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
if (beta > BETA_TABLE[n_y_lidar]) {
if (!(_sensorFault & SENSOR_LIDAR)) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar fault, beta %5.2f", double(beta));
mavlink_log_info(&mavlink_log_pub, "[lpe] lidar fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_LIDAR;
}
@@ -112,7 +112,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
} else if (_sensorFault & SENSOR_LIDAR) {
_sensorFault &= ~SENSOR_LIDAR;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar OK");
mavlink_log_info(&mavlink_log_pub, "[lpe] lidar OK");
}
// kalman filter correction always
@@ -128,7 +128,7 @@ void BlockLocalPositionEstimator::lidarCheckTimeout()
if (!(_sensorTimeout & SENSOR_LIDAR)) {
_sensorTimeout |= SENSOR_LIDAR;
_lidarStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar timeout ");
mavlink_log_info(&mavlink_log_pub, "[lpe] lidar timeout ");
}
}
}
src/modules/local_position_estimator/sensors/mocap.cpp
+15 -15
View File
@@ -25,14 +25,14 @@ void BlockLocalPositionEstimator::mocapInit()
// if finished
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
"%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
double(_mocapStats.getMean()(0)),
double(_mocapStats.getMean()(1)),
double(_mocapStats.getMean()(2)),
double(_mocapStats.getStdDev()(0)),
double(_mocapStats.getStdDev()(1)),
double(_mocapStats.getStdDev()(2)));
mavlink_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
"%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
double(_mocapStats.getMean()(0)),
double(_mocapStats.getMean()(1)),
double(_mocapStats.getMean()(2)),
double(_mocapStats.getStdDev()(0)),
double(_mocapStats.getStdDev()(1)),
double(_mocapStats.getStdDev()(2)));
_sensorTimeout &= ~SENSOR_MOCAP;
_sensorFault &= ~SENSOR_MOCAP;
@@ -43,8 +43,8 @@ void BlockLocalPositionEstimator::mocapInit()
if (!_map_ref.init_done && _is_global_cov_init && !_visionUpdated) {
// initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well)
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (mocap) : lat %6.2f lon %6.2f alt %5.1f m",
double(_ref_lat), double(_ref_lon), double(_ref_alt));
mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (mocap) : lat %6.2f lon %6.2f alt %5.1f m",
double(_ref_lat), double(_ref_lon), double(_ref_alt));
map_projection_init(&_map_ref, _ref_lat, _ref_lon);
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
@@ -106,8 +106,8 @@ void BlockLocalPositionEstimator::mocapCorrect()
Vector<float, n_y_mocap> y;
if (mocapMeasure(y) != OK) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", (double)_mocap_eph,
(double)_mocap_epv);
mavlink_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", (double)_mocap_eph,
(double)_mocap_epv);
return;
}
@@ -168,13 +168,13 @@ void BlockLocalPositionEstimator::mocapCorrect()
if (beta > BETA_TABLE[n_y_mocap]) {
if (!(_sensorFault & SENSOR_MOCAP)) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta));
//mavlink_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_MOCAP;
}
} else if (_sensorFault & SENSOR_MOCAP) {
_sensorFault &= ~SENSOR_MOCAP;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap OK");
//mavlink_log_info(&mavlink_log_pub, "[lpe] mocap OK");
}
// kalman filter correction always
@@ -190,7 +190,7 @@ void BlockLocalPositionEstimator::mocapCheckTimeout()
if (!(_sensorTimeout & SENSOR_MOCAP)) {
_sensorTimeout |= SENSOR_MOCAP;
_mocapStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap timeout ");
mavlink_log_info(&mavlink_log_pub, "[lpe] mocap timeout ");
}
}
}
src/modules/local_position_estimator/sensors/sonar.cpp
+5 -5
View File
@@ -26,11 +26,11 @@ void BlockLocalPositionEstimator::sonarInit()
// if finished
if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) {
if (_sonarStats.getStdDev()(0) > SONAR_MAX_INIT_STD) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init std > min");
mavlink_log_info(&mavlink_log_pub, "[lpe] sonar init std > min");
_sonarStats.reset();
} else if ((_timeStamp - _time_init_sonar) > SONAR_TIMEOUT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init timeout ");
mavlink_log_info(&mavlink_log_pub, "[lpe] sonar init timeout ");
_sonarStats.reset();
} else {
@@ -124,7 +124,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
if (beta > BETA_TABLE[n_y_sonar]) {
if (!(_sensorFault & SENSOR_SONAR)) {
_sensorFault |= SENSOR_SONAR;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta));
mavlink_log_info(&mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta));
}
// abort correction
@@ -132,7 +132,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
} else if (_sensorFault & SENSOR_SONAR) {
_sensorFault &= ~SENSOR_SONAR;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar OK");
//mavlink_log_info(&mavlink_log_pub, "[lpe] sonar OK");
}
// kalman filter correction if no fault
@@ -151,7 +151,7 @@ void BlockLocalPositionEstimator::sonarCheckTimeout()
if (!(_sensorTimeout & SENSOR_SONAR)) {
_sensorTimeout |= SENSOR_SONAR;
_sonarStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar timeout ");
mavlink_log_info(&mavlink_log_pub, "[lpe] sonar timeout ");
}
}
}
src/modules/local_position_estimator/sensors/vision.cpp
+14 -14
View File
@@ -30,14 +30,14 @@ void BlockLocalPositionEstimator::visionInit()
// increament sums for mean
if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: "
"%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
double(_visionStats.getMean()(0)),
double(_visionStats.getMean()(1)),
double(_visionStats.getMean()(2)),
double(_visionStats.getStdDev()(0)),
double(_visionStats.getStdDev()(1)),
double(_visionStats.getStdDev()(2)));
mavlink_log_info(&mavlink_log_pub, "[lpe] vision position init: "
"%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
double(_visionStats.getMean()(0)),
double(_visionStats.getMean()(1)),
double(_visionStats.getMean()(2)),
double(_visionStats.getStdDev()(0)),
double(_visionStats.getStdDev()(1)),
double(_visionStats.getStdDev()(2)));
_sensorTimeout &= ~SENSOR_VISION;
_sensorFault &= ~SENSOR_VISION;
@@ -48,8 +48,8 @@ void BlockLocalPositionEstimator::visionInit()
if (!_map_ref.init_done && _is_global_cov_init) {
// initialize global origin using the visual estimator reference
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m",
double(_ref_lat), double(_ref_lon), double(_ref_alt));
mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m",
double(_ref_lat), double(_ref_lon), double(_ref_alt));
map_projection_init(&_map_ref, _ref_lat, _ref_lon);
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
@@ -111,8 +111,8 @@ void BlockLocalPositionEstimator::visionCorrect()
Vector<float, n_y_vision> y;
if (visionMeasure(y) != OK) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", (double)_vision_eph,
(double)_vision_epv);
mavlink_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", (double)_vision_eph,
(double)_vision_epv);
return;
}
@@ -185,13 +185,13 @@ void BlockLocalPositionEstimator::visionCorrect()
if (beta > BETA_TABLE[n_y_vision]) {
if (!(_sensorFault & SENSOR_VISION)) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
mavlink_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_VISION;
}
} else if (_sensorFault & SENSOR_VISION) {
_sensorFault &= ~SENSOR_VISION;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK");
mavlink_log_info(&mavlink_log_pub, "[lpe] vision position OK");
}
// kalman filter correction if no fault