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

Build fix and airspeed console cal

Browse Source
This commit is contained in:
Lorenz Meier
2015-11-17 12:59:45 +01:00
parent 0509a5a9ea
commit 092a51426f
3 changed files with 23 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/airspeed_calibration.cpp
+20 -20
View File
@@ -68,7 +68,7 @@ static const char *sensor_name = "dpress";
static void feedback_calibration_failed(int mavlink_fd) static void feedback_calibration_failed(int mavlink_fd)
{ {
sleep(5); sleep(5);
mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
} }
int do_airspeed_calibration(int mavlink_fd) int do_airspeed_calibration(int mavlink_fd)
@@ -78,7 +78,7 @@ int do_airspeed_calibration(int mavlink_fd)
const unsigned maxcount = 2400; const unsigned maxcount = 2400;
/* give directions */ /* give directions */
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = (maxcount * 2) / 3; const unsigned calibration_count = (maxcount * 2) / 3;
@@ -101,7 +101,7 @@ int do_airspeed_calibration(int mavlink_fd)
paramreset_successful = true; paramreset_successful = true;
} else { } else {
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed"); mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
} }
px4_close(fd); px4_close(fd);
@@ -115,18 +115,18 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f; float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) { if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd"); mavlink_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
goto error_return; goto error_return;
} }
/* set scaling offset parameter */ /* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return; goto error_return;
} }
} }
mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind"); mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000); usleep(500 * 1000);
while (calibration_counter < calibration_count) { while (calibration_counter < calibration_count) {
@@ -149,7 +149,7 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++; calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) { if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
} }
} else if (poll_ret == 0) { } else if (poll_ret == 0) {
@@ -167,14 +167,14 @@ int do_airspeed_calibration(int mavlink_fd)
airscale.offset_pa = diff_pres_offset; airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) { if (fd_scale > 0) {
if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed"); mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
} }
px4_close(fd_scale); px4_close(fd_scale);
} }
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return; goto error_return;
} }
@@ -183,7 +183,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) { if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed"); warn("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
goto error_return; goto error_return;
} }
@@ -192,12 +192,12 @@ int do_airspeed_calibration(int mavlink_fd)
goto error_return; goto error_return;
} }
mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset); mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */ /* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000); usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "[cal] Create airflow now"); mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now");
calibration_counter = 0; calibration_counter = 0;
@@ -222,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) { if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", mavlink_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa); (int)diff_pres.differential_pressure_raw_pa);
} }
continue; continue;
@@ -230,26 +230,26 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */ /* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) { if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)", mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa); (int)diff_pres.differential_pressure_raw_pa);
mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!"); mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */ /* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f; diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return; goto error_return;
} }
/* save */ /* save */
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0); mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
(void)param_save_default(); (void)param_save_default();
feedback_calibration_failed(mavlink_fd); feedback_calibration_failed(mavlink_fd);
goto error_return; goto error_return;
} else { } else {
mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)", mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa); (int)diff_pres.differential_pressure_raw_pa);
break; break;
} }
@@ -266,9 +266,9 @@ int do_airspeed_calibration(int mavlink_fd)
goto error_return; goto error_return;
} }
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true); tune_neutral(true);
normal_return: normal_return:
src/modules/commander/commander.cpp
+2
View File
@@ -362,6 +362,8 @@ int commander_main(int argc, char *argv[])
calib_ret = do_level_calibration(mavlink_fd); calib_ret = do_level_calibration(mavlink_fd);
} else if (!strcmp(argv[2], "esc")) { } else if (!strcmp(argv[2], "esc")) {
calib_ret = do_esc_calibration(mavlink_fd, &armed); calib_ret = do_esc_calibration(mavlink_fd, &armed);
} else if (!strcmp(argv[2], "airspeed")) {
calib_ret = do_airspeed_calibration(mavlink_fd);
} else { } else {
warnx("argument %s unsupported.", argv[2]); warnx("argument %s unsupported.", argv[2]);
} }
src/modules/commander/state_machine_helper.cpp
+1 -1
View File
@@ -753,7 +753,7 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd,
bool checkAirspeed = false; bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not /* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */ * engaged and it's not a rotary wing */
if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol) { if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
checkAirspeed = true; checkAirspeed = true;
} }