dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Closing all opened file descriptors, fixed param save issue, tests clean

This commit is contained in:
Lorenz Meier 2013-09-01 12:47:10 +02:00
parent 9eff3170a3
commit 2d83c6f825
8 changed files with 57 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/commander/accelerometer_calibration.cpp
View File

@ -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]);

9
src/modules/commander/airspeed_calibration.cpp
View File

@ -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;
}
}

20
src/modules/commander/gyro_calibration.cpp
View File

@ -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;
}
}

44
src/modules/commander/mag_calibration.cpp
View File

@ -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;
}
}

3
src/modules/commander/rc_calibration.cpp
View File

@ -40,6 +40,7 @@
#include "commander_helper.h"
#include <poll.h>
#include <unistd.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <mavlink/mavlink_log.h>
@ -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;
}

11
src/modules/systemlib/param/param.c
View File

@ -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) {

7
src/modules/systemlib/rc_check.c
View File

@ -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;
}

7
src/systemcmds/preflight_check/preflight_check.c
View File

@ -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);