fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 04:13:33 +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-24 13:10:02 +01: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/position_estimator_inav/position_estimator_inav_main.cpp
+19 -20
View File
@@ -66,9 +66,9 @@
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <mavlink/mavlink_log.h>
#include <poll.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
@@ -234,8 +234,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
int mavlink_fd;
mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
orb_advert_t mavlink_log_pub = nullptr;
float x_est[2] = { 0.0f, 0.0f }; // pos, vel
float y_est[2] = { 0.0f, 0.0f }; // pos, vel
@@ -421,10 +420,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ret < 0) {
/* poll error */
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) {
wait_baro = false;
mavlink_log_info(mavlink_fd, "[inav] timed out waiting for a baro sample");
mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample");
}
else if (ret > 0) {
if (fds_init[0].revents & POLLIN) {
@@ -466,7 +465,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ret < 0) {
/* poll error */
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
continue;
} else if (ret > 0) {
@@ -549,9 +548,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
lidar.current_distance += params.lidar_calibration_offset;
}
if (updated) { //check if altitude estimation for lidar is enabled and new sensor data
if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance
&& (PX4_R(att.R, 2, 2) > 0.7f)) {
@@ -567,7 +566,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (lidar_first) {
lidar_first = false;
lidar_offset = dist_ground + z_est[0];
mavlink_log_info(mavlink_fd, "[inav] LIDAR: new ground offset");
mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset");
warnx("[inav] LIDAR: new ground offset");
}
@@ -764,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
last_vision_z = vision.z;
warnx("VISION estimate valid");
mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION estimate valid");
}
/* calculate correction for position */
@@ -818,7 +817,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
mocap_valid = true;
warnx("MOCAP data valid");
mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid");
mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data valid");
}
/* calculate correction for position */
@@ -842,7 +841,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (gps_valid) {
if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost");
warnx("[inav] GPS signal lost");
}
@@ -850,7 +849,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
gps_valid = true;
reset_est = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found");
warnx("[inav] GPS signal found");
}
}
@@ -883,7 +882,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
map_projection_init(&ref, lat, lon);
// XXX replace this print
warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
}
}
@@ -945,35 +944,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {
flow_valid = false;
warnx("FLOW timeout");
mavlink_log_info(mavlink_fd, "[inav] FLOW timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout");
}
/* check for timeout on GPS topic */
if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) {
gps_valid = false;
warnx("GPS timeout");
mavlink_log_info(mavlink_fd, "[inav] GPS timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout");
}
/* check for timeout on vision topic */
if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) {
vision_valid = false;
warnx("VISION timeout");
mavlink_log_info(mavlink_fd, "[inav] VISION timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout");
}
/* check for timeout on mocap topic */
if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) {
mocap_valid = false;
warnx("MOCAP timeout");
mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout");
}
/* check for lidar measurement timeout */
if (lidar_valid && (t > (lidar_time + lidar_timeout))) {
lidar_valid = false;
warnx("LIDAR timeout");
mavlink_log_info(mavlink_fd, "[inav] LIDAR timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout");
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
@@ -1380,7 +1379,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
warnx("stopped");
mavlink_log_info(mavlink_fd, "[inav] stopped");
mavlink_log_info(&mavlink_log_pub, "[inav] stopped");
thread_running = false;
return 0;
}