diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-11-03 18:26:02 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-11-03 18:26:02 +0100 |
commit | b53d86ed681eb6c9f979bb10d5f487fa9c94d81b (patch) | |
tree | 5b209b969d9c9092896c696f6e145dcd04691636 /src/modules/commander | |
parent | 8d4ec9b9cd7b6479217bab97694a1eae15b2cebb (diff) | |
download | px4-firmware-b53d86ed681eb6c9f979bb10d5f487fa9c94d81b.tar.gz px4-firmware-b53d86ed681eb6c9f979bb10d5f487fa9c94d81b.tar.bz2 px4-firmware-b53d86ed681eb6c9f979bb10d5f487fa9c94d81b.zip |
Hotfix for mag calibration
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/mag_calibration.cpp | 29 |
1 files changed, 19 insertions, 10 deletions
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 09f4104f8..4ebf266f4 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,7 +73,7 @@ int do_mag_calibration(int mavlink_fd) /* maximum 500 values */ const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; + unsigned int calibration_counter; struct mag_scale mscale_null = { 0.0f, @@ -99,28 +99,34 @@ int do_mag_calibration(int mavlink_fd) res = ioctl(fd, MAGIOCCALIBRATE, fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale"); + mavlink_log_critical(mavlink_fd, "Skipped scale calibration"); + /* this is non-fatal - mark it accordingly */ + res = OK; } } close(fd); - float *x; - float *y; - float *z; + float *x = NULL; + float *y = NULL; + float *z = NULL; if (res == OK) { /* allocate memory */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); - x = (float *)malloc(sizeof(float) * calibration_maxcount); - y = (float *)malloc(sizeof(float) * calibration_maxcount); - z = (float *)malloc(sizeof(float) * calibration_maxcount); + x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); + y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); + z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); if (x == NULL || y == NULL || z == NULL) { mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); res = ERROR; + return res; } + } else { + /* exit */ + return ERROR; } if (res == OK) { @@ -136,6 +142,8 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); + calibration_counter = 0; + while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -178,6 +186,7 @@ int do_mag_calibration(int mavlink_fd) float sphere_radius; if (res == OK) { + /* sphere fit */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); @@ -270,7 +279,7 @@ int do_mag_calibration(int mavlink_fd) } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } - - return res; } + + return res; } |