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

drivers/modules: changes after mavlink_log change

Browse Source 
The mavlink_log API changes lead to changes in all drivers/modules using
it.
This commit is contained in:
Julian Oes
2016-03-15 18:25:02 +00:00
parent b49b012d35
commit bba0d0384d
62 changed files with 731 additions and 831 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+42 -45
View File
@@ -1,5 +1,5 @@
#include "BlockLocalPositionEstimator.hpp"
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <matrix/math.hpp>
@@ -135,9 +135,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_time_last_vision_p(0),
_time_last_mocap(0),
// mavlink log
_mavlink_fd(open(MAVLINK_LOG_DEVICE, 0)),
// initialization flags
_baroInitialized(false),
_gpsInitialized(false),
@@ -342,7 +339,7 @@ void BlockLocalPositionEstimator::update()
_x(i) = 0;
}
mavlink_log_info(_mavlink_fd, "[lpe] reinit x");
mavlink_log_info(&_mavlink_log_pub, "[lpe] reinit x");
warnx("[lpe] reinit x");
}
@@ -361,7 +358,7 @@ void BlockLocalPositionEstimator::update()
}
if (reinit_P) {
mavlink_log_info(_mavlink_fd, "[lpe] reinit P");
mavlink_log_info(&_mavlink_log_pub, "[lpe] reinit P");
warnx("[lpe] reinit P");
initP();
}
@@ -471,7 +468,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_xy > EST_SRC_TIMEOUT) {
if (!_xyTimeout) {
_xyTimeout = true;
mavlink_log_info(_mavlink_fd, "[lpe] xy timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] xy timeout ");
warnx("[lpe] xy timeout ");
}
@@ -484,7 +481,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_z > EST_SRC_TIMEOUT) {
if (!_zTimeout) {
_zTimeout = true;
mavlink_log_info(_mavlink_fd, "[lpe] z timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] z timeout ");
warnx("[lpe] z timeout ");
}
@@ -497,7 +494,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_tz > EST_SRC_TIMEOUT) {
if (!_tzTimeout) {
_tzTimeout = true;
mavlink_log_info(_mavlink_fd, "[lpe] tz timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] tz timeout ");
warnx("[lpe] tz timeout ");
}
@@ -510,7 +507,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_baro > BARO_TIMEOUT) {
if (_baroInitialized) {
_baroInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] baro timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] baro timeout ");
warnx("[lpe] baro timeout ");
}
}
@@ -518,7 +515,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_gps > GPS_TIMEOUT) {
if (_gpsInitialized) {
_gpsInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] GPS timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] GPS timeout ");
warnx("[lpe] GPS timeout ");
}
}
@@ -526,7 +523,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
if (_flowInitialized) {
_flowInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] flow timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow timeout ");
warnx("[lpe] flow timeout ");
}
}
@@ -534,7 +531,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_sonar > RANGER_TIMEOUT) {
if (_sonarInitialized) {
_sonarInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] sonar timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar timeout ");
warnx("[lpe] sonar timeout ");
}
}
@@ -542,7 +539,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_lidar > RANGER_TIMEOUT) {
if (_lidarInitialized) {
_lidarInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] lidar timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar timeout ");
warnx("[lpe] lidar timeout ");
}
}
@@ -550,7 +547,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) {
if (_visionInitialized) {
_visionInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] vision position timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position timeout ");
warnx("[lpe] vision position timeout ");
}
}
@@ -558,7 +555,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) {
if (_mocapInitialized) {
_mocapInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] mocap timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap timeout ");
warnx("[lpe] mocap timeout ");
}
}
@@ -574,7 +571,7 @@ void BlockLocalPositionEstimator::updateHome()
// like gps and baro to be off, need to allow it
// to reset by resetting covariance
initP();
mavlink_log_info(_mavlink_fd, "[lpe] home "
mavlink_log_info(&_mavlink_log_pub, "[lpe] home "
"lat %6.2f lon %6.2f alt %5.1f m",
lat, lon, double(alt));
warnx("[lpe] home "
@@ -598,7 +595,7 @@ void BlockLocalPositionEstimator::initBaro()
if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
_baroAltHome = _baroStats.getMean()(0);
mavlink_log_info(_mavlink_fd,
mavlink_log_info(&_mavlink_log_pub,
"[lpe] baro init %d m std %d cm",
(int)_baroStats.getMean()(0),
(int)(100 * _baroStats.getStdDev()(0)));
@@ -643,7 +640,7 @@ void BlockLocalPositionEstimator::initGps()
_gpsAltHome = _gpsStats.getMean()(2);
map_projection_init(&_map_ref,
_gpsLatHome, _gpsLonHome);
mavlink_log_info(_mavlink_fd, "[lpe] gps init "
mavlink_log_info(&_mavlink_log_pub, "[lpe] gps init "
"lat %6.2f lon %6.2f alt %5.1f m",
_gpsLatHome,
_gpsLonHome,
@@ -691,7 +688,7 @@ void BlockLocalPositionEstimator::initLidar()
}
// not, might want to hard code this to zero
mavlink_log_info(_mavlink_fd, "[lpe] lidar init: "
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)));
@@ -731,7 +728,7 @@ void BlockLocalPositionEstimator::initSonar()
}
// not, might want to hard code this to zero
mavlink_log_info(_mavlink_fd, "[lpe] sonar init "
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar init "
"mean %d cm std %d cm",
int(100 * _sonarStats.getMean()(0)),
int(100 * _sonarStats.getStdDev()(0)));
@@ -758,7 +755,7 @@ void BlockLocalPositionEstimator::initFlow()
_time_last_flow = _timeStamp;
if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) {
mavlink_log_info(_mavlink_fd, "[lpe] flow init: "
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow init: "
"quality %d std %d",
int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0)));
@@ -785,7 +782,7 @@ void BlockLocalPositionEstimator::initVision()
if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
_visionHome = _visionStats.getMean();
mavlink_log_info(_mavlink_fd, "[lpe] vision position init: "
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)),
@@ -825,7 +822,7 @@ void BlockLocalPositionEstimator::initMocap()
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
_mocapHome = _mocapStats.getMean();
mavlink_log_info(_mavlink_fd, "[lpe] mocap position init: "
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)),
@@ -1071,7 +1068,7 @@ void BlockLocalPositionEstimator::correctFlow()
if (qual < _flow_min_q.get()) {
if (_flowFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] low flow quality %d", int(qual));
mavlink_log_info(&_mavlink_log_pub, "[lpe] low flow quality %d", int(qual));
warnx("[lpe] low flow quality %d", int(qual));
_flowFault = FAULT_SEVERE;
}
@@ -1096,7 +1093,7 @@ void BlockLocalPositionEstimator::correctFlow()
} else {
// no valid distance sensor, so return
if (_flowFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] no distance for flow");
mavlink_log_info(&_mavlink_log_pub, "[lpe] no distance for flow");
warnx("[lpe] no distance for flow");
_flowFault = FAULT_SEVERE;
}
@@ -1167,14 +1164,14 @@ void BlockLocalPositionEstimator::correctFlow()
if (beta > BETA_TABLE[n_y_flow]) {
if (_flowFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] flow fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
warnx("[lpe] flow fault, beta %5.2f", double(beta));
_flowFault = FAULT_MINOR;
}
} else if (_flowFault) {
_flowFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] flow OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow OK");
warnx("[lpe] flow OK");
}
@@ -1207,7 +1204,7 @@ void BlockLocalPositionEstimator::correctSonar()
} else if (d > max_dist) {
if (_sonarFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] sonar max distance");
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar max distance");
warnx("[lpe] sonar max distance");
_sonarFault = FAULT_SEVERE;
}
@@ -1262,7 +1259,7 @@ void BlockLocalPositionEstimator::correctSonar()
if (_sonarFault < FAULT_MINOR) {
// avoid printing messages near ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] sonar fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta));
warnx("[lpe] sonar fault, beta %5.2f", double(beta));
}
@@ -1274,7 +1271,7 @@ void BlockLocalPositionEstimator::correctSonar()
// avoid printing messages near ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] sonar OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar OK");
warnx("[lpe] sonar OK");
}
}
@@ -1324,7 +1321,7 @@ void BlockLocalPositionEstimator::correctBaro()
if (beta > BETA_TABLE[n_y_baro]) {
if (_baroFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] baro fault, r %5.2f m, beta %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
warnx("[lpe] baro fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
@@ -1333,7 +1330,7 @@ void BlockLocalPositionEstimator::correctBaro()
} else if (_baroFault) {
_baroFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] baro OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] baro OK");
warnx("[lpe] baro OK");
}
@@ -1369,7 +1366,7 @@ void BlockLocalPositionEstimator::correctLidar()
} else if (d > max_dist) {
if (_lidarFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar out of range");
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar out of range");
warnx("[lpe] lidar out of range");
_lidarFault = FAULT_SEVERE;
}
@@ -1416,7 +1413,7 @@ void BlockLocalPositionEstimator::correctLidar()
// only print message if above 1 meter, avoids
// message clutter when on ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar fault, r %5.2f m, beta %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
warnx("[lpe] lidar fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
@@ -1431,7 +1428,7 @@ void BlockLocalPositionEstimator::correctLidar()
// only print message if above 1 meter, avoids
// message clutter when on ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar OK");
warnx("[lpe] lidar OK");
}
}
@@ -1461,7 +1458,7 @@ void BlockLocalPositionEstimator::correctGps()
if (nSat < 6 || eph > _gps_eph_max.get()) {
if (_gpsFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph));
mavlink_log_info(&_mavlink_log_pub, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph));
warnx("[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph));
_gpsFault = FAULT_SEVERE;
}
@@ -1554,12 +1551,12 @@ void BlockLocalPositionEstimator::correctGps()
if (beta > BETA_TABLE[n_y_gps]) {
if (_gpsFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] gps fault, beta: %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta));
warnx("[lpe] gps fault, beta: %5.2f", double(beta));
mavlink_log_info(_mavlink_fd, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
double(r(0)), double(r(1)), double(r(2)),
double(r(3)), double(r(4)), double(r(5)));
mavlink_log_info(_mavlink_fd, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)),
double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5)));
warnx("[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
@@ -1573,7 +1570,7 @@ void BlockLocalPositionEstimator::correctGps()
} else if (_gpsFault) {
_gpsFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] GPS OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] GPS OK");
warnx("[lpe] GPS OK");
}
@@ -1618,14 +1615,14 @@ void BlockLocalPositionEstimator::correctVision()
if (beta > BETA_TABLE[n_y_vision]) {
if (_visionFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] vision position fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
warnx("[lpe] vision position fault, beta %5.2f", double(beta));
_visionFault = FAULT_MINOR;
}
} else if (_visionFault) {
_visionFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] vision position OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position OK");
warnx("[lpe] vision position OK");
}
@@ -1672,14 +1669,14 @@ void BlockLocalPositionEstimator::correctMocap()
if (beta > BETA_TABLE[n_y_mocap]) {
if (_mocapFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] mocap fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta));
warnx("[lpe] mocap fault, beta %5.2f", double(beta));
_mocapFault = FAULT_MINOR;
}
} else if (_mocapFault) {
_mocapFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] mocap OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap OK");
warnx("[lpe] mocap OK");
}