fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 18:11:36 +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/commander/airspeed_calibration.cpp
+35 -35
View File
@@ -52,7 +52,7 @@
#include <drivers/drv_airspeed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/airspeed.h>
@@ -65,20 +65,20 @@ static const int ERROR = -1;
static const char *sensor_name = "dpress";
static void feedback_calibration_failed(int mavlink_fd)
static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
{
sleep(5);
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
int do_airspeed_calibration(int mavlink_fd)
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
{
int result = OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 2400;
/* give directions */
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = (maxcount * 2) / 3;
@@ -101,12 +101,12 @@ int do_airspeed_calibration(int mavlink_fd)
paramreset_successful = true;
} else {
mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed");
}
px4_close(fd);
}
int cancel_sub = calibrate_cancel_subscribe();
if (!paramreset_successful) {
@@ -115,26 +115,26 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
}
mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
@@ -149,12 +149,12 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
@@ -167,14 +167,14 @@ int do_airspeed_calibration(int mavlink_fd)
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
@@ -183,28 +183,28 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
goto error_return;
}
} else {
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Create airflow now");
calibration_counter = 0;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
goto error_return;
}
@@ -222,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
@@ -230,56 +230,56 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
/* save */
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
(void)param_save_default();
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
} else {
mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
normal_return:
calibrate_cancel_unsubscribe(cancel_sub);
px4_close(diff_pres_sub);
// This give a chance for the log messages to go out of the queue before someone else stomps on then
sleep(1);
return result;
error_return:
result = ERROR;
goto normal_return;