Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

position_estimator_inav cosmetic changes

Browse Source
This commit is contained in:
Anton Babushkin
2013-06-13 06:49:17 +04:00
parent 4860c73008
commit 4cdee2be03
1 changed files with 9 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+9 -7
View File
@@ -152,7 +152,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
warnx("started."); warnx("started.");
int mavlink_fd; int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); mavlink_log_info(mavlink_fd, "[inav] started");
/* initialize values */ /* initialize values */
float x_est[3] = { 0.0f, 0.0f, 0.0f }; float x_est[3] = { 0.0f, 0.0f, 0.0f };
@@ -241,8 +241,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
map_projection_init(lat_current, lon_current); map_projection_init(lat_current, lon_current);
/* publish global position messages only after first GPS message */ /* publish global position messages only after first GPS message */
} }
warnx("initialized projection with: lat = %.10f, lon = %.10f", lat_current, lon_current);
warnx("initialized projection with: lat: %.10f, lon:%.10f", lat_current, lon_current); mavlink_log_info(mavlink_fd, "[inav] home: lat = %.10f, lon = %.10f", lat_current, lon_current);
hrt_abstime t_prev = 0; hrt_abstime t_prev = 0;
thread_running = true; thread_running = true;
@@ -269,7 +269,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
{ .fd = sensor_combined_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN },
{ .fd = vehicle_gps_position_sub, .events = POLLIN } { .fd = vehicle_gps_position_sub, .events = POLLIN }
}; };
printf("[position_estimator_inav] main loop started\n"); warnx("main loop started.");
while (!thread_should_exit) { while (!thread_should_exit) {
bool accelerometer_updated = false; bool accelerometer_updated = false;
@@ -337,7 +337,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else { } else {
baro_alt0 /= (float)(baro_loop_cnt); baro_alt0 /= (float)(baro_loop_cnt);
local_flag_baroINITdone = true; local_flag_baroINITdone = true;
mavlink_log_info(mavlink_fd, "[position_estimator_inav] baro_alt0 = %.2f", baro_alt0); warnx("baro_alt0 = %.2f", baro_alt0);
mavlink_log_info(mavlink_fd, "[inav] baro_alt0 = %.2f", baro_alt0);
pos.home_alt = baro_alt0;
} }
} }
} }
@@ -428,7 +430,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (t - updates_counter_start > updates_counter_len) { if (t - updates_counter_start > updates_counter_len) {
float updates_dt = (t - updates_counter_start) * 0.000001f; float updates_dt = (t - updates_counter_start) * 0.000001f;
printf( printf(
"[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", "[inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n",
accel_updates / updates_dt, accel_updates / updates_dt,
baro_updates / updates_dt, baro_updates / updates_dt,
gps_updates / updates_dt, gps_updates / updates_dt,
@@ -462,7 +464,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
warnx("exiting."); warnx("exiting.");
mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting"); mavlink_log_info(mavlink_fd, "[inav] exiting");
thread_running = false; thread_running = false;
return 0; return 0;
} }