aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander/mag_calibration.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander/mag_calibration.cpp')
-rw-r--r--src/modules/commander/mag_calibration.cpp179
1 files changed, 99 insertions, 80 deletions
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 7147fb283..1d2f38437 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -63,93 +63,121 @@ static const int ERROR = -1;
static const char *sensor_name = "mag";
+int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id);
+
int do_mag_calibration(int mavlink_fd)
{
- int32_t device_id;
- mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
- mavlink_log_info(mavlink_fd, "don't move system");
-
- /* 45 seconds */
- uint64_t calibration_interval = 45 * 1000 * 1000;
+ const unsigned max_mags = 3;
- /* maximum 500 values */
- const unsigned int calibration_maxcount = 240;
- unsigned int calibration_counter;
+ int32_t device_id[max_mags];
+ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+ sleep(1);
- struct mag_scale mscale_null = {
+ struct mag_scale mscale_null[max_mags] = {
+ {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
- };
+ }
+ } ;
- int res = OK;
+ int res = ERROR;
- /* erase old calibration */
- int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+ char str[30];
- device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+ for (unsigned s = 0; s < max_mags; s++) {
- res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
+ mavlink_log_info(mavlink_fd, "Magnetometer #%u", s);
+ sleep(3);
- if (res != OK) {
- mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
- }
+ /* erase old calibration */
+ (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
+ int fd = open(str, O_RDONLY);
- if (res == OK) {
- /* calibrate range */
- res = ioctl(fd, MAGIOCCALIBRATE, fd);
+ if (fd < 0) {
+ continue;
+ }
+
+ device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
+
+ /* ensure all scale fields are initialized tha same as the first struct */
+ (void)memcpy(&mscale_null[s], &mscale_null[0], sizeof(mscale_null[0]));
+
+ res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null[s]);
if (res != OK) {
- mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
- /* this is non-fatal - mark it accordingly */
- res = OK;
+ mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+ }
+
+ if (res == OK) {
+ /* calibrate range */
+ res = ioctl(fd, MAGIOCCALIBRATE, fd);
+
+ if (res != OK) {
+ mavlink_log_info(mavlink_fd, "Skipped scale calibration");
+ /* this is non-fatal - mark it accordingly */
+ res = OK;
+ }
+ }
+
+ close(fd);
+
+ if (res == OK) {
+ res = calibrate_instance(mavlink_fd, s, device_id[s]);
}
}
- close(fd);
+ return res;
+}
+
+int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
+{
+ /* 45 seconds */
+ uint64_t calibration_interval = 45 * 1000 * 1000;
+
+ /* maximum 500 values */
+ const unsigned int calibration_maxcount = 240;
+ unsigned int calibration_counter;
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);
+ char str[30];
+ int res = ERROR;
+
+ /* allocate memory */
+ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
- 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));
+ 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");
+ if (x == NULL || y == NULL || z == NULL) {
+ mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
- /* clean up */
- if (x != NULL) {
- free(x);
- }
-
- if (y != NULL) {
- free(y);
- }
+ /* clean up */
+ if (x != NULL) {
+ free(x);
+ }
- if (z != NULL) {
- free(z);
- }
+ if (y != NULL) {
+ free(y);
+ }
- res = ERROR;
- return res;
+ if (z != NULL) {
+ free(z);
}
- } else {
- /* exit */
- return ERROR;
+ res = ERROR;
+ return res;
}
if (res == OK) {
- int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
+ int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s);
if (sub_mag < 0) {
mavlink_log_critical(mavlink_fd, "No mag found, abort");
@@ -239,8 +267,8 @@ int do_mag_calibration(int mavlink_fd)
if (res == OK) {
/* apply calibration and set parameters */
struct mag_scale mscale;
-
- fd = open(MAG_DEVICE_PATH, 0);
+ (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
+ int fd = open(str, 0);
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
if (res != OK) {
@@ -262,35 +290,26 @@ int do_mag_calibration(int mavlink_fd)
close(fd);
if (res == OK) {
- /* set parameters */
- if (param_set(param_find("CAL_MAG0_ID"), &(device_id))) {
- res = ERROR;
- }
- if (param_set(param_find("CAL_MAG0_XOFF"), &(mscale.x_offset))) {
- res = ERROR;
- }
- if (param_set(param_find("CAL_MAG0_YOFF"), &(mscale.y_offset))) {
- res = ERROR;
- }
-
- if (param_set(param_find("CAL_MAG0_ZOFF"), &(mscale.z_offset))) {
- res = ERROR;
- }
-
- if (param_set(param_find("CAL_MAG0_XSCALE"), &(mscale.x_scale))) {
- res = ERROR;
- }
-
- if (param_set(param_find("CAL_MAG0_YSCALE"), &(mscale.y_scale))) {
- res = ERROR;
- }
-
- if (param_set(param_find("CAL_MAG0_ZSCALE"), &(mscale.z_scale))) {
+ bool failed = false;
+ /* set parameters */
+ (void)sprintf(str, "CAL_MAG%u_ID", s);
+ failed |= (OK != param_set(param_find(str), &(device_id)));
+ (void)sprintf(str, "CAL_MAG%u_XOFF", s);
+ failed |= (OK != param_set(param_find(str), &(mscale.x_offset)));
+ (void)sprintf(str, "CAL_MAG%u_YOFF", s);
+ failed |= (OK != param_set(param_find(str), &(mscale.y_offset)));
+ (void)sprintf(str, "CAL_MAG%u_ZOFF", s);
+ failed |= (OK != param_set(param_find(str), &(mscale.z_offset)));
+ (void)sprintf(str, "CAL_MAG%u_XSCALE", s);
+ failed |= (OK != param_set(param_find(str), &(mscale.x_scale)));
+ (void)sprintf(str, "CAL_MAG%u_YSCALE", s);
+ failed |= (OK != param_set(param_find(str), &(mscale.y_scale)));
+ (void)sprintf(str, "CAL_MAG%u_ZSCALE", s);
+ failed |= (OK != param_set(param_find(str), &(mscale.z_scale)));
+
+ if (failed) {
res = ERROR;
- }
-
- if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-19 03:01:06 +0000