int uORBTest::UnitTest::test_multi() { /* this routine tests the multi-topic support */ test_note("try multi-topic support"); struct orb_test t {}, u {}; t.val = 0; int instance0; _pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); test_note("advertised"); int instance1; _pfd[1] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); if (instance0 != 0) { return test_fail("mult. id0: %d", instance0); } if (instance1 != 1) { return test_fail("mult. id1: %d", instance1); } t.val = 103; if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[0], &t)) { return test_fail("mult. pub0 fail"); } test_note("published"); t.val = 203; if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[1], &t)) { return test_fail("mult. pub1 fail"); } /* subscribe to both topics and ensure valid data is received */ int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) { return test_fail("sub #0 copy failed: %d", errno); } if (u.val != 103) { return test_fail("sub #0 val. mismatch: %d", u.val); } int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) { return test_fail("sub #1 copy failed: %d", errno); } if (u.val != 203) { return test_fail("sub #1 val. mismatch: %d", u.val); } /* test priorities */ int prio; if (PX4_OK != orb_priority(sfd0, &prio)) { return test_fail("prio #0"); } if (prio != ORB_PRIO_MAX) { return test_fail("prio: %d", prio); } if (PX4_OK != orb_priority(sfd1, &prio)) { return test_fail("prio #1"); } if (prio != ORB_PRIO_MIN) { return test_fail("prio: %d", prio); } if (PX4_OK != latency_test<struct orb_test>(ORB_ID(orb_test), false)) { return test_fail("latency test failed"); } orb_unsubscribe(sfd0); orb_unsubscribe(sfd1); return test_note("PASS multi-topic test"); }
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) { calibrate_return result = calibrate_return_ok; mag_worker_data_t worker_data; worker_data.mavlink_log_pub = mavlink_log_pub; worker_data.done_count = 0; worker_data.calibration_points_perside = calibration_total_points / calibration_sides; worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; // Collect: Right-side up, Left Side, Nose down worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false; worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false; worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false; worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true; worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true; calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN)); usleep(100000); calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN)); usleep(100000); calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN)); usleep(100000); calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN)); usleep(100000); calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_RIGHT)); usleep(100000); calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_RIGHT)); usleep(100000); for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { // Initialize to no subscription worker_data.sub_mag[cur_mag] = -1; // Initialize to no memory allocated worker_data.x[cur_mag] = NULL; worker_data.y[cur_mag] = NULL; worker_data.z[cur_mag] = NULL; worker_data.calibration_counter_total[cur_mag] = 0; } const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside; char str[30]; for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) { calibration_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory"); result = calibrate_return_error; } } // Setup subscriptions to mag sensors if (result == calibrate_return_ok) { // We should not try to subscribe if the topic doesn't actually exist and can be counted. const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag)); for (unsigned cur_mag = 0; cur_mag < mag_count; cur_mag++) { // Mag in this slot is available worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag); #ifdef __PX4_QURT // For QURT respectively the driver framework, we need to get the device ID by copying one report. struct mag_report mag_report; orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report); device_ids[cur_mag] = mag_report.device_id; #endif if (worker_data.sub_mag[cur_mag] < 0) { calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag); result = calibrate_return_error; break; } if (device_ids[cur_mag] != 0) { // Get priority int32_t prio; orb_priority(worker_data.sub_mag[cur_mag], &prio); if (prio > device_prio_max) { device_prio_max = prio; device_id_primary = device_ids[cur_mag]; } } else { calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag); result = calibrate_return_error; break; } } } // Limit update rate to get equally spaced measurements over time (in ms) if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside; //calibration_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs); orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs); } } } if (result == calibrate_return_ok) { int cancel_sub = calibrate_cancel_subscribe(); result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output cancel_sub, // Subscription to vehicle_command for cancel support worker_data.side_data_collected, // Sides to calibrate mag_calibration_worker, // Calibration worker &worker_data, // Opaque data for calibration worked true); // true: lenient still detection calibrate_cancel_unsubscribe(cancel_sub); } // Close subscriptions for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (worker_data.sub_mag[cur_mag] >= 0) { px4_close(worker_data.sub_mag[cur_mag]); } } // Calculate calibration values for each mag float sphere_x[max_mags]; float sphere_y[max_mags]; float sphere_z[max_mags]; float sphere_radius[max_mags]; // Sphere fit the data to get calibration values if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available and we should have values for it to calibrate sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], worker_data.calibration_counter_total[cur_mag], 100, 0.0f, &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag], &sphere_radius[cur_mag]); if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) { calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag); result = calibrate_return_error; } if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN || fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN || fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) { calibration_log_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external"); calibration_log_info(mavlink_log_pub, "Excessive offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag], (double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag); result = calibrate_return_ok; } } } } // Print uncalibrated data points if (result == calibrate_return_ok) { // DO NOT REMOVE! Critical validation data! // printf("RAW DATA:\n--------------------\n"); // for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { // if (worker_data.calibration_counter_total[cur_mag] == 0) { // continue; // } // printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); // for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { // float x = worker_data.x[cur_mag][i]; // float y = worker_data.y[cur_mag][i]; // float z = worker_data.z[cur_mag][i]; // printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); // } // printf(">>>>>>>\n"); // } // printf("CALIBRATED DATA:\n--------------------\n"); // for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { // if (worker_data.calibration_counter_total[cur_mag] == 0) { // continue; // } // printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); // for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { // float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag]; // float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag]; // float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag]; // printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); // } // printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]); // printf(">>>>>>>\n"); // } } // Data points are no longer needed for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { free(worker_data.x[cur_mag]); free(worker_data.y[cur_mag]); free(worker_data.z[cur_mag]); } if (result == calibrate_return_ok) { (void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary)); for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { struct mag_calibration_s mscale; #ifndef __PX4_QURT int fd_mag = -1; // Set new scale (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); fd_mag = px4_open(str, 0); if (fd_mag < 0) { calibration_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag); result = calibrate_return_error; } if (result == calibrate_return_ok) { if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) { calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag); result = calibrate_return_error; } } #endif if (result == calibrate_return_ok) { mscale.x_offset = sphere_x[cur_mag]; mscale.y_offset = sphere_y[cur_mag]; mscale.z_offset = sphere_z[cur_mag]; #ifndef __PX4_QURT if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag); result = calibrate_return_error; } #endif } #ifndef __PX4_QURT // Mag device no longer needed if (fd_mag >= 0) { px4_close(fd_mag); } #endif if (result == calibrate_return_ok) { bool failed = false; /* set parameters */ (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); // FIXME: scaling is not used right now on QURT #ifndef __PX4_QURT failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); #endif if (failed) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag); result = calibrate_return_error; } else { calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga", cur_mag, (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); #ifndef __PX4_QURT calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f", cur_mag, (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); #endif usleep(200000); } } } } } return result; }
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors) { calibrate_return result = calibrate_return_ok; *active_sensors = 0; accel_worker_data_t worker_data; worker_data.mavlink_log_pub = mavlink_log_pub; worker_data.done_count = 0; bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false }; // Initialize subs to error condition so we know which ones are open and which are not for (size_t i=0; i<max_accel_sens; i++) { worker_data.subs[i] = -1; } uint64_t timestamps[max_accel_sens]; // We should not try to subscribe if the topic doesn't actually exist and can be counted. const unsigned accel_count = orb_group_count(ORB_ID(sensor_accel)); for (unsigned i = 0; i < accel_count; i++) { worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); if (worker_data.subs[i] < 0) { result = calibrate_return_error; break; } #if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI) // For QURT respectively the driver framework, we need to get the device ID by copying one report. struct accel_report accel_report; orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report); device_id[i] = accel_report.device_id; #endif /* store initial timestamp - used to infer which sensors are active */ struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); timestamps[i] = arp.timestamp; if (device_id[i] != 0) { // Get priority int32_t prio; orb_priority(worker_data.subs[i], &prio); if (prio > device_prio_max) { device_prio_max = prio; device_id_primary = device_id[i]; } } else { calibration_log_critical(mavlink_log_pub, "[cal] Accel #%u no device id, abort", i); result = calibrate_return_error; break; } } if (result == calibrate_return_ok) { int cancel_sub = calibrate_cancel_subscribe(); result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */); calibrate_cancel_unsubscribe(cancel_sub); } /* close all subscriptions */ for (unsigned i = 0; i < max_accel_sens; i++) { if (worker_data.subs[i] >= 0) { /* figure out which sensors were active */ struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { (*active_sensors)++; } px4_close(worker_data.subs[i]); } } if (result == calibrate_return_ok) { /* calculate offsets and transform matrix */ for (unsigned i = 0; i < (*active_sensors); i++) { result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (result != calibrate_return_ok) { calibration_log_critical(mavlink_log_pub, "[cal] ERROR: calibration calculation error"); break; } } } return result; }
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors) { calibrate_return result = calibrate_return_ok; *active_sensors = 0; accel_worker_data_t worker_data; worker_data.mavlink_log_pub = mavlink_log_pub; worker_data.done_count = 0; bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false }; // Initialise sub to sensor thermal compensation data worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); // Initialize subs to error condition so we know which ones are open and which are not for (size_t i=0; i<max_accel_sens; i++) { worker_data.subs[i] = -1; } uint64_t timestamps[max_accel_sens] = {}; // We should not try to subscribe if the topic doesn't actually exist and can be counted. const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel)); // Warn that we will not calibrate more than max_accels accelerometers if (orb_accel_count > max_accel_sens) { calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count, max_accel_sens); } for (unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel < max_accel_sens; cur_accel++) { // Lock in to correct ORB instance bool found_cur_accel = false; for(unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) { worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i); struct accel_report report = {}; orb_copy(ORB_ID(sensor_accel), worker_data.subs[cur_accel], &report); #ifdef __PX4_NUTTX // For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL // and match it up with the one from the uORB subscription, because the // instance ordering of uORB and the order of the FDs may not be the same. if(report.device_id == device_id[cur_accel]) { // Device IDs match, correct ORB instance for this accel found_cur_accel = true; // store initial timestamp - used to infer which sensors are active timestamps[cur_accel] = report.timestamp; } else { orb_unsubscribe(worker_data.subs[cur_accel]); } #else // For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report. device_id[cur_accel] = report.device_id; found_cur_accel = true; #endif } if(!found_cur_accel) { calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]); result = calibrate_return_error; break; } if (device_id[cur_accel] != 0) { // Get priority int32_t prio; orb_priority(worker_data.subs[cur_accel], &prio); if (prio > device_prio_max) { device_prio_max = prio; device_id_primary = device_id[cur_accel]; } } else { calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel); result = calibrate_return_error; break; } } if (result == calibrate_return_ok) { int cancel_sub = calibrate_cancel_subscribe(); result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */); calibrate_cancel_unsubscribe(cancel_sub); } /* close all subscriptions */ for (unsigned i = 0; i < max_accel_sens; i++) { if (worker_data.subs[i] >= 0) { /* figure out which sensors were active */ struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { (*active_sensors)++; } px4_close(worker_data.subs[i]); } } orb_unsubscribe(worker_data.sensor_correction_sub); if (result == calibrate_return_ok) { /* calculate offsets and transform matrix */ for (unsigned i = 0; i < (*active_sensors); i++) { result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (result != calibrate_return_ok) { calibration_log_critical(mavlink_log_pub, "ERROR: calibration calculation error"); break; } } } return result; }
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) { calibrate_return result = calibrate_return_ok; mag_worker_data_t worker_data; worker_data.mavlink_log_pub = mavlink_log_pub; worker_data.done_count = 0; worker_data.calibration_points_perside = calibration_total_points / calibration_sides; worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; // Collect: As defined by configuration // start with a full mask, all six bits set uint32_t cal_mask = (1 << 6) - 1; param_get(param_find("CAL_MAG_SIDES"), &cal_mask); calibration_sides = 0; for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) / sizeof(worker_data.side_data_collected[0])); i++) { if ((cal_mask & (1 << i)) > 0) { // mark as missing worker_data.side_data_collected[i] = false; calibration_sides++; } else { // mark as completed from the beginning worker_data.side_data_collected[i] = true; calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(static_cast<enum detect_orientation_return>(i))); usleep(100000); } } for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { // Initialize to no subscription worker_data.sub_mag[cur_mag] = -1; // Initialize to no memory allocated worker_data.x[cur_mag] = nullptr; worker_data.y[cur_mag] = nullptr; worker_data.z[cur_mag] = nullptr; worker_data.calibration_counter_total[cur_mag] = 0; } const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside; char str[30]; for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); if (worker_data.x[cur_mag] == nullptr || worker_data.y[cur_mag] == nullptr || worker_data.z[cur_mag] == nullptr) { calibration_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory"); result = calibrate_return_error; } } // Setup subscriptions to mag sensors if (result == calibrate_return_ok) { // We should not try to subscribe if the topic doesn't actually exist and can be counted. const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag)); // Warn that we will not calibrate more than max_mags magnetometers if (mag_count > max_mags) { calibration_log_critical(mavlink_log_pub, "[cal] Detected %u mags, but will calibrate only %u", mag_count, max_mags); } for (unsigned cur_mag = 0; cur_mag < mag_count && cur_mag < max_mags; cur_mag++) { // Mag in this slot is available worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag); #if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP) // For QURT respectively the driver framework, we need to get the device ID by copying one report. struct mag_report mag_report; orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report); device_ids[cur_mag] = mag_report.device_id; #endif if (worker_data.sub_mag[cur_mag] < 0) { calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag); result = calibrate_return_error; break; } if (device_ids[cur_mag] != 0) { // Get priority int32_t prio; orb_priority(worker_data.sub_mag[cur_mag], &prio); if (prio > device_prio_max) { device_prio_max = prio; device_id_primary = device_ids[cur_mag]; } } else { calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag); result = calibrate_return_error; break; } } } // Limit update rate to get equally spaced measurements over time (in ms) if (result == calibrate_return_ok) { for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside; //calibration_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs); orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs); } } } if (result == calibrate_return_ok) { int cancel_sub = calibrate_cancel_subscribe(); result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output cancel_sub, // Subscription to vehicle_command for cancel support worker_data.side_data_collected, // Sides to calibrate mag_calibration_worker, // Calibration worker &worker_data, // Opaque data for calibration worked true); // true: lenient still detection calibrate_cancel_unsubscribe(cancel_sub); } // Close subscriptions for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { if (worker_data.sub_mag[cur_mag] >= 0) { px4_close(worker_data.sub_mag[cur_mag]); } } // Calculate calibration values for each mag float sphere_x[max_mags]; float sphere_y[max_mags]; float sphere_z[max_mags]; float sphere_radius[max_mags]; float diag_x[max_mags]; float diag_y[max_mags]; float diag_z[max_mags]; float offdiag_x[max_mags]; float offdiag_y[max_mags]; float offdiag_z[max_mags]; for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { sphere_x[cur_mag] = 0.0f; sphere_y[cur_mag] = 0.0f; sphere_z[cur_mag] = 0.0f; sphere_radius[cur_mag] = 0.2f; diag_x[cur_mag] = 1.0f; diag_y[cur_mag] = 1.0f; diag_z[cur_mag] = 1.0f; offdiag_x[cur_mag] = 0.0f; offdiag_y[cur_mag] = 0.0f; offdiag_z[cur_mag] = 0.0f; } // Sphere fit the data to get calibration values if (result == calibrate_return_ok) { for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available and we should have values for it to calibrate ellipsoid_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], worker_data.calibration_counter_total[cur_mag], 100, 0.0f, &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag], &sphere_radius[cur_mag], &diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag], &offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]); result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag], sphere_radius[cur_mag], diag_x[cur_mag], diag_y[cur_mag], diag_z[cur_mag], offdiag_x[cur_mag], offdiag_y[cur_mag], offdiag_z[cur_mag], mavlink_log_pub, cur_mag); if (result == calibrate_return_error) { break; } } } } // Print uncalibrated data points if (result == calibrate_return_ok) { // DO NOT REMOVE! Critical validation data! // printf("RAW DATA:\n--------------------\n"); // for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { // if (worker_data.calibration_counter_total[cur_mag] == 0) { // continue; // } // printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); // for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { // float x = worker_data.x[cur_mag][i]; // float y = worker_data.y[cur_mag][i]; // float z = worker_data.z[cur_mag][i]; // printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); // } // printf(">>>>>>>\n"); // } // printf("CALIBRATED DATA:\n--------------------\n"); // for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { // if (worker_data.calibration_counter_total[cur_mag] == 0) { // continue; // } // printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); // for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { // float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag]; // float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag]; // float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag]; // printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); // } // printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]); // printf(">>>>>>>\n"); // } } // Data points are no longer needed for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { free(worker_data.x[cur_mag]); free(worker_data.y[cur_mag]); free(worker_data.z[cur_mag]); } if (result == calibrate_return_ok) { for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { struct mag_calibration_s mscale; mscale.x_scale = 1.0; mscale.y_scale = 1.0; mscale.z_scale = 1.0; #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) int fd_mag = -1; // Set new scale (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); fd_mag = px4_open(str, 0); if (fd_mag < 0) { calibration_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag); result = calibrate_return_error; } if (result == calibrate_return_ok) { if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != PX4_OK) { calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag); result = calibrate_return_error; } } #endif if (result == calibrate_return_ok) { mscale.x_offset = sphere_x[cur_mag]; mscale.y_offset = sphere_y[cur_mag]; mscale.z_offset = sphere_z[cur_mag]; mscale.x_scale = diag_x[cur_mag]; mscale.y_scale = diag_y[cur_mag]; mscale.z_scale = diag_z[cur_mag]; #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != PX4_OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag); result = calibrate_return_error; } #endif } #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) // Mag device no longer needed if (fd_mag >= 0) { px4_close(fd_mag); } #endif if (result == calibrate_return_ok) { bool failed = false; /* set parameters */ (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); // FIXME: scaling is not used right now on QURT #ifndef __PX4_QURT (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); #endif if (failed) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag); result = calibrate_return_error; } else { calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga", cur_mag, (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); #ifndef __PX4_QURT calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f", cur_mag, (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); #endif usleep(200000); } } } } // Trigger a param set on the last step so the whole // system updates (void)param_set(param_find("CAL_MAG_PRIME"), &(device_id_primary)); } return result; }