From 2d83c6f825db02f04624467f3d0b492ee371c72a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 12:47:10 +0200 Subject: Closing all opened file descriptors, fixed param save issue, tests clean --- .../commander/accelerometer_calibration.cpp | 4 +- src/modules/commander/airspeed_calibration.cpp | 9 +++-- src/modules/commander/gyro_calibration.cpp | 20 ++++++---- src/modules/commander/mag_calibration.cpp | 44 ++++++---------------- src/modules/commander/rc_calibration.cpp | 3 ++ src/modules/systemlib/param/param.c | 11 +++++- src/modules/systemlib/rc_check.c | 7 +++- src/systemcmds/preflight_check/preflight_check.c | 7 ++++ 8 files changed, 57 insertions(+), 48 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ed6707f04..cfa7d9e8a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -241,8 +241,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; int orient = detect_orientation(mavlink_fd, sensor_combined_sub); - if (orient < 0) + if (orient < 0) { + close(sensor_combined_sub); return ERROR; + } if (data_collected[orient]) { mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index e414e5f70..248eb4a66 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -85,6 +85,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + close(diff_pres_sub); return ERROR; } } @@ -95,6 +96,7 @@ int do_airspeed_calibration(int mavlink_fd) if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + close(diff_pres_sub); return ERROR; } @@ -104,18 +106,17 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + close(diff_pres_sub); return ERROR; } - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - + close(diff_pres_sub); return OK; } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + close(diff_pres_sub); return ERROR; } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 33566d4e5..82a0349c9 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -60,12 +60,12 @@ int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); - const int calibration_count = 5000; + const unsigned calibration_count = 5000; int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; - int calibration_counter = 0; + unsigned calibration_counter = 0; float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; /* set offsets to zero */ @@ -101,6 +101,8 @@ int do_gyro_calibration(int mavlink_fd) gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[2] += raw.gyro_rad_s[2]; calibration_counter++; + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); } else { poll_errcount++; @@ -108,6 +110,7 @@ int do_gyro_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); + close(sub_sensor_combined); return ERROR; } } @@ -147,24 +150,23 @@ int do_gyro_calibration(int mavlink_fd) if (save_ret != 0) { warnx("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, "gyro store failed"); + close(sub_sensor_combined); return ERROR; } - mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_neutral(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); + close(sub_sensor_combined); return ERROR; } /*** --- SCALING --- ***/ - mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x"); - mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); + mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); unsigned rotations_count = 30; @@ -187,8 +189,8 @@ int do_gyro_calibration(int mavlink_fd) /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) && (hrt_absolute_time() - start_time > 5 * 1e6)) { - mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); - mavlink_log_info(mavlink_fd, "gyro calibration done"); + mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); + close(sub_sensor_combined); return OK; } @@ -281,9 +283,11 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "gyro calibration done"); /* third beep by cal end routine */ + close(sub_sensor_combined); return OK; } else { mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + close(sub_sensor_combined); return ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index e38616027..b0d4266be 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -142,7 +142,6 @@ int do_mag_calibration(int mavlink_fd) axis_index++; mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); axis_deadline += calibration_interval / 3; @@ -152,14 +151,6 @@ int do_mag_calibration(int mavlink_fd) break; } - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { @@ -169,30 +160,10 @@ int do_mag_calibration(int mavlink_fd) y[calibration_counter] = mag.y; z[calibration_counter] = mag.z; - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - calibration_counter++; + if (calibration_counter % (calibration_maxcount / 20) == 0) + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount); + } else { poll_errcount++; @@ -200,6 +171,10 @@ int do_mag_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); + close(sub_mag); + free(x); + free(y); + free(z); return ERROR; } @@ -211,7 +186,9 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; + mavlink_log_info(mavlink_fd, "mag cal progress <70> percent"); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + mavlink_log_info(mavlink_fd, "mag cal progress <80> percent"); free(x); free(y); @@ -269,6 +246,7 @@ int do_mag_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + close(sub_mag); return ERROR; } @@ -288,11 +266,13 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "magnetometer calibration completed"); mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); + close(sub_mag); return OK; /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + close(sub_mag); return ERROR; } } diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index fe87a3323..9fc1d6470 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -40,6 +40,7 @@ #include "commander_helper.h" #include +#include #include #include #include @@ -80,9 +81,11 @@ int do_rc_calibration(int mavlink_fd) if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + close(sub_man); return ERROR; } mavlink_log_info(mavlink_fd, "trim calibration done"); + close(sub_man); return OK; } diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 69a9bdf9b..24b52d1a9 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -505,8 +505,15 @@ param_get_default_file(void) int param_save_default(void) { + int result; + /* delete the file in case it exists */ - unlink(param_get_default_file()); + struct stat buffer; + if (stat(param_get_default_file(), &buffer) == 0) { + result = unlink(param_get_default_file()); + if (result != OK) + warnx("unlinking file %s failed.", param_get_default_file()); + } /* create the file */ int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); @@ -516,7 +523,7 @@ param_save_default(void) return -1; } - int result = param_export(fd, false); + result = param_export(fd, false); close(fd); if (result != 0) { diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index f3a2aee9e..60d6473b8 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -66,7 +66,7 @@ int rc_calibration_check(void) { // count++; // } - + int channel_fail_count = 0; for (int i = 0; i < RC_CHANNELS_MAX; i++) { /* should the channel be enabled? */ @@ -142,7 +142,12 @@ int rc_calibration_check(void) { /* sanity checks pass, enable channel */ if (count) { mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); usleep(100000); } + + channel_fail_count += count; } + + return channel_fail_count; } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index a323261cc..f5bd01ce8 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -142,6 +142,10 @@ int preflight_check_main(int argc, char *argv[]) bool rc_ok = (OK == rc_calibration_check()); + /* warn */ + if (!rc_ok) + warnx("rc calibration test failed"); + /* require RC ok to keep system_ok */ system_ok &= rc_ok; @@ -156,6 +160,9 @@ system_eval: } else { fflush(stdout); + warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); + fflush(stderr); + int buzzer = open("/dev/tone_alarm", O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); -- cgit v1.2.3