diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2013-11-03 18:38:34 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-11-03 18:38:34 +0100 |
commit | fa4533e3592d055bbd298d81ad3760e19dedec95 (patch) | |
tree | 6e0bf72ca6d08671b54266696404471a2db6fddd /src/modules | |
parent | 1aef7c502cc50b724a3eb47c05dcdb5bec6de4a3 (diff) | |
parent | 791695ccd008859f6abe1a12d86b7be2ba811fec (diff) | |
download | px4-firmware-fa4533e3592d055bbd298d81ad3760e19dedec95.tar.gz px4-firmware-fa4533e3592d055bbd298d81ad3760e19dedec95.tar.bz2 px4-firmware-fa4533e3592d055bbd298d81ad3760e19dedec95.zip |
Merge remote-tracking branch 'upstream/master' into fw_autoland_att_tecs
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/mag_calibration.cpp | 29 | ||||
-rw-r--r-- | src/modules/uORB/uORB.cpp | 4 |
2 files changed, 22 insertions, 11 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; } diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 7abbf42ae..149b8f6d2 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -249,8 +249,10 @@ ORBDevNode::close(struct file *filp) } else { SubscriberData *sd = filp_to_sd(filp); - if (sd != nullptr) + if (sd != nullptr) { + hrt_cancel(&sd->update_call); delete sd; + } } return CDev::close(filp); |