// Returns calibrate_return_error if any parameter is not finite // Logs if parameters are out of range static calibrate_return check_calibration_result(float offset_x, float offset_y, float offset_z, float sphere_radius, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z, orb_advert_t *mavlink_log_pub, size_t cur_mag) { float must_be_finite[] = {offset_x, offset_y, offset_z, sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z}; float should_be_not_huge[] = {offset_x, offset_y, offset_z}; float should_be_positive[] = {sphere_radius, diag_x, diag_y, diag_z}; // Make sure every parameter is finite const int num_finite = sizeof(must_be_finite) / sizeof(*must_be_finite); for (unsigned i = 0; i < num_finite; ++i) { if (!PX4_ISFINITE(must_be_finite[i])) { calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag); return calibrate_return_error; } } // Notify if offsets are too large const int num_not_huge = sizeof(should_be_not_huge) / sizeof(*should_be_not_huge); for (unsigned i = 0; i < num_not_huge; ++i) { if (fabsf(should_be_not_huge[i]) > MAG_MAX_OFFSET_LEN) { calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external"); break; } } // Notify if a parameter which should be positive is non-positive const int num_positive = sizeof(should_be_positive) / sizeof(*should_be_positive); for (unsigned i = 0; i < num_positive; ++i) { if (should_be_positive[i] <= 0.0f) { calibration_log_critical(mavlink_log_pub, "Warning: %s mag with non-positive scale", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external"); break; } } return calibrate_return_ok; }
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) { int result = PX4_OK; unsigned calibration_counter = 0; const unsigned maxcount = 2400; /* give directions */ calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); const unsigned calibration_count = (maxcount * 2) / 3; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; float diff_pres_offset = 0.0f; /* Reset sensor parameters */ struct airspeed_scale airscale = { diff_pres_offset, 1.0f, }; bool paramreset_successful = false; int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); if (fd > 0) { if (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; } else { calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed"); } px4_close(fd); } int cancel_sub = calibrate_cancel_subscribe(); if (!paramreset_successful) { /* only warn if analog scaling is zero */ float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found"); goto error_return; } /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } } calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind"); usleep(500 * 1000); while (calibration_counter < calibration_count) { if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) { goto error_return; } /* wait blocking for new data */ px4_pollfd_struct_t fds[1]; fds[0].fd = diff_pres_sub; fds[0].events = POLLIN; int poll_ret = px4_poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; /* any differential pressure failure a reason to abort */ if (diff_pres.error_count != 0) { calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%llu)", diff_pres.error_count); calibration_log_critical(mavlink_log_pub, "[cal] Check your wiring before trying again"); feedback_calibration_failed(mavlink_log_pub); goto error_return; } if (calibration_counter % (calibration_count / 20) == 0) { calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_log_pub); goto error_return; } } diff_pres_offset = diff_pres_offset / calibration_count; if (PX4_ISFINITE(diff_pres_offset)) { int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0); airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed"); } px4_close(fd_scale); } // Prevent a completely zero param // since this is used to detect a missing calibration // This value is numerically down in the noise and has // no effect on the sensor performance. if (fabsf(diff_pres_offset) < 0.00000001f) { diff_pres_offset = 0.00000001f; } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } } else { feedback_calibration_failed(mavlink_log_pub); goto error_return; } calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); calibration_log_critical(mavlink_log_pub, "[cal] Blow across front of pitot without touching"); calibration_counter = 0; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) { goto error_return; } /* wait blocking for new data */ px4_pollfd_struct_t fds[1]; fds[0].fd = diff_pres_sub; fds[0].events = POLLIN; int poll_ret = px4_poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) { if (diff_pres.differential_pressure_filtered_pa > 0) { calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa); break; } else { /* do not allow negative values */ calibration_log_info(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa); calibration_log_info(mavlink_log_pub, "[cal] Swap static and dynamic ports!"); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ diff_pres_offset = 0.0f; if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } /* save */ calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); param_save_default(); feedback_calibration_failed(mavlink_log_pub); goto error_return; } } if (calibration_counter % 500 == 0) { calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_filtered_pa); tune_neutral(true); } calibration_counter++; } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_log_pub); goto error_return; } } if (calibration_counter == maxcount) { feedback_calibration_failed(mavlink_log_pub); goto error_return; } calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); tune_neutral(true); /* Wait 2sec for the airflow to stop and ensure the driver filter has caught up, otherwise * the followup preflight checks might fail. */ usleep(2e6); normal_return: calibrate_cancel_unsubscribe(cancel_sub); px4_close(diff_pres_sub); // This give a chance for the log messages to go out of the queue before someone else stomps on then sleep(1); return result; error_return: result = PX4_ERROR; goto normal_return; }
static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub) { sleep(5); calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); }
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; }
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { calibrate_return result = calibrate_return_ok; unsigned int calibration_counter_side; mag_worker_data_t* worker_data = (mag_worker_data_t*)(data); calibration_log_info(worker_data->mavlink_log_pub, "[cal] Rotate vehicle around the detected orientation"); calibration_log_info(worker_data->mavlink_log_pub, "[cal] Continue rotation for %s %u s", detect_orientation_str(orientation), worker_data->calibration_interval_perside_seconds); /* * Detect if the system is rotating. * * We're detecting this as a general rotation on any axis, not necessary on the one we * asked the user for. This is because we really just need two roughly orthogonal axes * for a good result, so we're not constraining the user more than we have to. */ hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 5; hrt_abstime last_gyro = 0; float gyro_x_integral = 0.0f; float gyro_y_integral = 0.0f; float gyro_z_integral = 0.0f; const float gyro_int_thresh_rad = 0.5f; int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro)); while (fabsf(gyro_x_integral) < gyro_int_thresh_rad && fabsf(gyro_y_integral) < gyro_int_thresh_rad && fabsf(gyro_z_integral) < gyro_int_thresh_rad) { /* abort on request */ if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) { result = calibrate_return_cancelled; px4_close(sub_gyro); return result; } /* abort with timeout */ if (hrt_absolute_time() > detection_deadline) { result = calibrate_return_error; warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral); calibration_log_critical(worker_data->mavlink_log_pub, "Failed: This calibration requires rotation."); break; } /* Wait clocking for new data on all gyro */ px4_pollfd_struct_t fds[1]; fds[0].fd = sub_gyro; fds[0].events = POLLIN; size_t fd_count = 1; int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { struct gyro_report gyro; orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro); /* ensure we have a valid first timestamp */ if (last_gyro > 0) { /* integrate */ float delta_t = (gyro.timestamp - last_gyro) / 1e6f; gyro_x_integral += gyro.x * delta_t; gyro_y_integral += gyro.y * delta_t; gyro_z_integral += gyro.z * delta_t; } last_gyro = gyro.timestamp; } } px4_close(sub_gyro); uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; unsigned poll_errcount = 0; calibration_counter_side = 0; while (hrt_absolute_time() < calibration_deadline && calibration_counter_side < worker_data->calibration_points_perside) { if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) { result = calibrate_return_cancelled; break; } // Wait clocking for new data on all mags px4_pollfd_struct_t fds[max_mags]; size_t fd_count = 0; for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { if (worker_data->sub_mag[cur_mag] >= 0 && device_ids[cur_mag] != 0) { fds[fd_count].fd = worker_data->sub_mag[cur_mag]; fds[fd_count].events = POLLIN; fd_count++; } } int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { int prev_count[max_mags]; bool rejected = false; for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag]; if (worker_data->sub_mag[cur_mag] >= 0) { struct mag_report mag; orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); // Check if this measurement is good to go in rejected = rejected || reject_sample(mag.x, mag.y, mag.z, worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], worker_data->calibration_counter_total[cur_mag], calibration_sides * worker_data->calibration_points_perside); worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x; worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y; worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z; worker_data->calibration_counter_total[cur_mag]++; } } // Keep calibration of all mags in lockstep if (rejected) { // Reset counts, since one of the mags rejected the measurement for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag]; } } else { calibration_counter_side++; unsigned new_progress = progress_percentage(worker_data) + (unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)); if (new_progress - _last_mag_progress > 3) { // Progress indicator for side calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side calibration: progress <%u>", detect_orientation_str(orientation), new_progress); usleep(20000); _last_mag_progress = new_progress; } } } else { poll_errcount++; } if (poll_errcount > worker_data->calibration_points_perside * 3) { result = calibrate_return_error; calibration_log_info(worker_data->mavlink_log_pub, CAL_ERROR_SENSOR_MSG); break; } } if (result == calibrate_return_ok) { calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation)); worker_data->done_count++; usleep(20000); calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data)); } return result; }
int do_mag_calibration(orb_advert_t *mavlink_log_pub) { calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); struct mag_calibration_s mscale_null; mscale_null.x_offset = 0.0f; mscale_null.x_scale = 1.0f; mscale_null.y_offset = 0.0f; mscale_null.y_scale = 1.0f; mscale_null.z_offset = 0.0f; mscale_null.z_scale = 1.0f; int result = OK; // Determine which mags are available and reset each char str[30]; for (size_t i=0; i < max_mags; i++) { device_ids[i] = 0; // signals no mag } _last_mag_progress = 0; for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { #ifndef __PX4_QURT // Reset mag id to mag not available (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); result = param_set_no_notification(param_find(str), &(device_ids[cur_mag])); if (result != OK) { calibration_log_info(mavlink_log_pub, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag); break; } #else (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); result = param_set(param_find(str), &mscale_null.x_offset); if (result != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); result = param_set(param_find(str), &mscale_null.y_offset); if (result != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); result = param_set(param_find(str), &mscale_null.z_offset); if (result != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); result = param_set(param_find(str), &mscale_null.x_scale); if (result != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); result = param_set(param_find(str), &mscale_null.y_scale); if (result != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); result = param_set(param_find(str), &mscale_null.z_scale); if (result != OK) { PX4_ERR("unable to reset %s", str); } #endif /* for calibration, commander will run on apps, so orb messages are used to get info from dsp */ #ifndef __PX4_QURT // Attempt to open mag (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); int fd = px4_open(str, O_RDONLY); if (fd < 0) { continue; } // Get device id for this mag device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); internal[cur_mag] = (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0); // Reset mag scale result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); if (result != OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, cur_mag); } /* calibrate range */ if (result == OK) { result = px4_ioctl(fd, MAGIOCCALIBRATE, fd); if (result != OK) { calibration_log_info(mavlink_log_pub, "[cal] Skipped scale calibration, sensor %u", cur_mag); /* this is non-fatal - mark it accordingly */ result = OK; } } px4_close(fd); #endif } // Calibrate all mags at the same time if (result == OK) { switch (mag_calibrate_all(mavlink_log_pub)) { case calibrate_return_cancelled: // Cancel message already displayed, we're done here result = ERROR; break; case calibrate_return_ok: /* auto-save to EEPROM */ result = param_save_default(); /* if there is a any preflight-check system response, let the barrage of messages through */ usleep(200000); if (result == OK) { calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); usleep(20000); calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); usleep(20000); break; } else { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG); usleep(20000); } // Fall through default: calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); usleep(20000); break; } } /* give this message enough time to propagate */ usleep(600000); return result; }
int do_level_calibration(orb_advert_t *mavlink_log_pub) { const unsigned cal_time = 5; const unsigned cal_hz = 100; unsigned settle_time = 30; bool success = false; int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level"); param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF"); param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF"); param_t board_rot_handle = param_find("SENS_BOARD_ROT"); // save old values if calibration fails float roll_offset_current; float pitch_offset_current; int32_t board_rot_current = 0; param_get(roll_offset_handle, &roll_offset_current); param_get(pitch_offset_handle, &pitch_offset_current); param_get(board_rot_handle, &board_rot_current); // give attitude some time to settle if there have been changes to the board rotation parameters if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON ) { settle_time = 0; } float zero = 0.0f; param_set_no_notification(roll_offset_handle, &zero); param_set_no_notification(pitch_offset_handle, &zero); param_notify_changes(); px4_pollfd_struct_t fds[1]; fds[0].fd = att_sub; fds[0].events = POLLIN; float roll_mean = 0.0f; float pitch_mean = 0.0f; unsigned counter = 0; // sleep for some time hrt_abstime start = hrt_absolute_time(); while(hrt_elapsed_time(&start) < settle_time * 1000000) { calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time)); sleep(settle_time / 10); } start = hrt_absolute_time(); // average attitude for 5 seconds while(hrt_elapsed_time(&start) < cal_time * 1000000) { int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); if (pollret <= 0) { // attitude estimator is not running calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot"); calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level"); goto out; } orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); matrix::Eulerf euler = matrix::Quatf(att.q); roll_mean += euler.phi(); pitch_mean += euler.theta(); counter++; } calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); if (counter > (cal_time * cal_hz / 2 )) { roll_mean /= counter; pitch_mean /= counter; } else { calibration_log_info(mavlink_log_pub, "not enough measurements taken"); success = false; goto out; } if (fabsf(roll_mean) > 0.8f ) { calibration_log_critical(mavlink_log_pub, "excess roll angle"); } else if (fabsf(pitch_mean) > 0.8f ) { calibration_log_critical(mavlink_log_pub, "excess pitch angle"); } else { roll_mean *= (float)M_RAD_TO_DEG; pitch_mean *= (float)M_RAD_TO_DEG; param_set_no_notification(roll_offset_handle, &roll_mean); param_set_no_notification(pitch_offset_handle, &pitch_mean); param_notify_changes(); success = true; } out: if (success) { calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level"); return 0; } else { // set old parameters param_set_no_notification(roll_offset_handle, &roll_offset_current); param_set_no_notification(pitch_offset_handle, &pitch_offset_current); param_notify_changes(); calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level"); return 1; } }
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; }
int do_accel_calibration(orb_advert_t *mavlink_log_pub) { #ifdef __PX4_NUTTX int fd; #endif calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); struct accel_calibration_s accel_scale; accel_scale.x_offset = 0.0f; accel_scale.x_scale = 1.0f; accel_scale.y_offset = 0.0f; accel_scale.y_scale = 1.0f; accel_scale.z_offset = 0.0f; accel_scale.z_scale = 1.0f; int res = PX4_OK; char str[30]; /* reset all sensors */ for (unsigned s = 0; s < max_accel_sens; s++) { #ifdef __PX4_NUTTX sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ fd = px4_open(str, 0); if (fd < 0) { continue; } device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); px4_close(fd); if (res != PX4_OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s); } #else (void)sprintf(str, "CAL_ACC%u_XOFF", s); res = param_set_no_notification(param_find(str), &accel_scale.x_offset); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_YOFF", s); res = param_set_no_notification(param_find(str), &accel_scale.y_offset); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_ZOFF", s); res = param_set_no_notification(param_find(str), &accel_scale.z_offset); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_XSCALE", s); res = param_set_no_notification(param_find(str), &accel_scale.x_scale); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_YSCALE", s); res = param_set_no_notification(param_find(str), &accel_scale.y_scale); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_ZSCALE", s); res = param_set_no_notification(param_find(str), &accel_scale.z_scale); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } param_notify_changes(); #endif } float accel_offs[max_accel_sens][3]; float accel_T[max_accel_sens][3][3]; unsigned active_sensors = 0; /* measure and calculate offsets & scales */ if (res == PX4_OK) { calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors); if (cal_return == calibrate_return_cancelled) { // Cancel message already displayed, nothing left to do return PX4_ERROR; } else if (cal_return == calibrate_return_ok) { res = PX4_OK; } else { res = PX4_ERROR; } } if (res != PX4_OK) { if (active_sensors == 0) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG); } return PX4_ERROR; } /* measurements completed successfully, rotate calibration values */ param_t board_rotation_h = param_find("SENS_BOARD_ROT"); int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; math::Matrix<3, 3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) { /* handle individual sensors, one by one */ math::Vector<3> accel_offs_vec(accel_offs[uorb_index]); math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; math::Matrix<3, 3> accel_T_mat(accel_T[uorb_index]); math::Matrix<3, 3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_scale = accel_T_rotated(0, 0); accel_scale.y_offset = accel_offs_rotated(1); accel_scale.y_scale = accel_T_rotated(1, 1); accel_scale.z_offset = accel_offs_rotated(2); accel_scale.z_scale = accel_T_rotated(2, 2); bool failed = false; failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, (double)accel_scale.x_offset, (double)accel_scale.y_offset, (double)accel_scale.z_offset); PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, (double)accel_scale.x_scale, (double)accel_scale.y_scale, (double)accel_scale.z_scale); /* check if thermal compensation is enabled */ int32_t tc_enabled_int; param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int)); if (tc_enabled_int == 1) { /* Get struct containing sensor thermal compensation data */ struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */ memset(&sensor_correction, 0, sizeof(sensor_correction)); int sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction); orb_unsubscribe(sensor_correction_sub); /* don't allow a parameter instance to be calibrated more than once by another uORB instance */ if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) { tc_locked[sensor_correction.accel_mapping[uorb_index]] = true; /* update the _X0_ terms to include the additional offset */ int32_t handle; float val; for (unsigned axis_index = 0; axis_index < 3; axis_index++) { val = 0.0f; (void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index); handle = param_find(str); param_get(handle, &val); if (axis_index == 0) { val += accel_scale.x_offset; } else if (axis_index == 1) { val += accel_scale.y_offset; } else if (axis_index == 2) { val += accel_scale.z_offset; } failed |= (PX4_OK != param_set_no_notification(handle, &val)); } /* update the _SCL_ terms to include the scale factor */ for (unsigned axis_index = 0; axis_index < 3; axis_index++) { val = 1.0f; (void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index); handle = param_find(str); if (axis_index == 0) { val = accel_scale.x_scale; } else if (axis_index == 1) { val = accel_scale.y_scale; } else if (axis_index == 2) { val = accel_scale.z_scale; } failed |= (PX4_OK != param_set_no_notification(handle, &val)); } param_notify_changes(); } // Ensure the calibration values used by the driver are at default settings when we are using thermal calibration data accel_scale.x_offset = 0.f; accel_scale.y_offset = 0.f; accel_scale.z_offset = 0.f; accel_scale.x_scale = 1.f; accel_scale.y_scale = 1.f; accel_scale.z_scale = 1.f; } // save the driver level calibration data (void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); (void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset))); (void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset))); (void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale))); (void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale))); (void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale))); (void)sprintf(str, "CAL_ACC%u_ID", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index]))); if (failed) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, uorb_index); return PX4_ERROR; } #ifdef __PX4_NUTTX sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index); fd = px4_open(str, 0); if (fd < 0) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist"); res = PX4_ERROR; } else { res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); px4_close(fd); } if (res != PX4_OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index); } #endif } if (res == PX4_OK) { /* if there is a any preflight-check system response, let the barrage of messages through */ usleep(200000); calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); } else { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); } /* give this message enough time to propagate */ usleep(600000); return res; }
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; }
int do_accel_calibration(orb_advert_t *mavlink_log_pub) { #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) int fd; #endif calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); struct accel_calibration_s accel_scale; accel_scale.x_offset = 0.0f; accel_scale.x_scale = 1.0f; accel_scale.y_offset = 0.0f; accel_scale.y_scale = 1.0f; accel_scale.z_offset = 0.0f; accel_scale.z_scale = 1.0f; int res = OK; char str[30]; /* reset all sensors */ for (unsigned s = 0; s < max_accel_sens; s++) { #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ fd = px4_open(str, 0); if (fd < 0) { continue; } device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); px4_close(fd); if (res != OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s); } #else (void)sprintf(str, "CAL_ACC%u_XOFF", s); res = param_set(param_find(str), &accel_scale.x_offset); if (res != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_YOFF", s); res = param_set(param_find(str), &accel_scale.y_offset); if (res != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_ZOFF", s); res = param_set(param_find(str), &accel_scale.z_offset); if (res != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_XSCALE", s); res = param_set(param_find(str), &accel_scale.x_scale); if (res != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_YSCALE", s); res = param_set(param_find(str), &accel_scale.y_scale); if (res != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_ZSCALE", s); res = param_set(param_find(str), &accel_scale.z_scale); if (res != OK) { PX4_ERR("unable to reset %s", str); } #endif } float accel_offs[max_accel_sens][3]; float accel_T[max_accel_sens][3][3]; unsigned active_sensors; /* measure and calculate offsets & scales */ if (res == OK) { calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors); if (cal_return == calibrate_return_cancelled) { // Cancel message already displayed, nothing left to do return ERROR; } else if (cal_return == calibrate_return_ok) { res = OK; } else { res = ERROR; } } if (res != OK) { if (active_sensors == 0) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG); } return ERROR; } /* measurements completed successfully, rotate calibration values */ param_t board_rotation_h = param_find("SENS_BOARD_ROT"); int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; math::Matrix<3, 3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); for (unsigned i = 0; i < active_sensors; i++) { /* handle individual sensors, one by one */ math::Vector<3> accel_offs_vec(accel_offs[i]); math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; math::Matrix<3, 3> accel_T_mat(accel_T[i]); math::Matrix<3, 3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_scale = accel_T_rotated(0, 0); accel_scale.y_offset = accel_offs_rotated(1); accel_scale.y_scale = accel_T_rotated(1, 1); accel_scale.z_offset = accel_offs_rotated(2); accel_scale.z_scale = accel_T_rotated(2, 2); bool failed = false; failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", i, (double)accel_scale.x_offset, (double)accel_scale.y_offset, (double)accel_scale.z_offset); PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", i, (double)accel_scale.x_scale, (double)accel_scale.y_scale, (double)accel_scale.z_scale); /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); (void)sprintf(str, "CAL_ACC%u_YOFF", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset))); (void)sprintf(str, "CAL_ACC%u_ZOFF", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset))); (void)sprintf(str, "CAL_ACC%u_XSCALE", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale))); (void)sprintf(str, "CAL_ACC%u_YSCALE", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale))); (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale))); (void)sprintf(str, "CAL_ACC%u_ID", i); failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i]))); if (failed) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, i); return ERROR; } #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i); fd = px4_open(str, 0); if (fd < 0) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist"); res = ERROR; } else { res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); px4_close(fd); } if (res != OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, i); } #endif } if (res == OK) { /* auto-save to EEPROM */ res = param_save_default(); if (res != OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG); } /* if there is a any preflight-check system response, let the barrage of messages through */ usleep(200000); calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); } else { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); } /* give this message enough time to propagate */ usleep(600000); return res; }
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; }
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed) { int return_code = OK; int fd = -1; struct battery_status_s battery; int batt_sub = -1; bool batt_updated = false; bool batt_connected = false; hrt_abstime battery_connect_wait_timeout = 30000000; hrt_abstime pwm_high_timeout = 4000000; hrt_abstime timeout_start; calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); batt_sub = orb_subscribe(ORB_ID(battery_status)); if (batt_sub < 0) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery"); goto Error; } // Make sure battery is disconnected orb_copy(ORB_ID(battery_status), batt_sub, &battery); if (battery.voltage_filtered_v > 3.0f) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again"); goto Error; } armed->in_esc_calibration_mode = true; fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); if (fd < 0) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device"); goto Error; } /* tell IO/FMU that its ok to disable its safety with the switch */ if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch"); goto Error; } /* tell IO/FMU that the system is armed (it will output values if safety is off) */ if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system"); goto Error; } /* tell IO to switch off safety without using the safety switch */ if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off"); goto Error; } calibration_log_info(mavlink_log_pub, "[cal] Connect battery now"); timeout_start = hrt_absolute_time(); while (true) { // We are either waiting for the user to connect the battery. Or we are waiting to let the PWM // sit high. hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout; if (hrt_absolute_time() - timeout_start > timeout_wait) { if (!batt_connected) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery"); goto Error; } // PWM was high long enough break; } if (!batt_connected) { orb_check(batt_sub, &batt_updated); if (batt_updated) { orb_copy(ORB_ID(battery_status), batt_sub, &battery); if (battery.voltage_filtered_v > 3.0f) { // Battery is connected, signal to user and start waiting again batt_connected = true; timeout_start = hrt_absolute_time(); calibration_log_info(mavlink_log_pub, "[cal] Battery connected"); } } } usleep(50000); } Out: if (batt_sub != -1) { orb_unsubscribe(batt_sub); } if (fd != -1) { if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) { calibration_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still off"); } if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != OK) { calibration_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Servos still armed"); } if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) { calibration_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still deactivated"); } px4_close(fd); } armed->in_esc_calibration_mode = false; if (return_code == OK) { calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc"); } return return_code; Error: return_code = ERROR; goto Out; }
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker, void *worker_data, bool lenient_still_position) { calibrate_return result = calibrate_return_ok; // Setup subscriptions to onboard accel sensor int sub_accel = orb_subscribe(ORB_ID(sensor_combined)); if (sub_accel < 0) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel"); return calibrate_return_error; } unsigned orientation_failures = 0; // Rotate through all requested orientation while (true) { if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) { result = calibrate_return_cancelled; break; } if (orientation_failures > 4) { result = calibrate_return_error; calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion"); break; } unsigned int side_complete_count = 0; // Update the number of completed sides for (unsigned i = 0; i < detect_orientation_side_count; i++) { if (side_data_collected[i]) { side_complete_count++; } } if (side_complete_count == detect_orientation_side_count) { // We have completed all sides, move on break; } /* inform user which orientations are still needed */ char pendingStr[80]; pendingStr[0] = 0; for (unsigned int cur_orientation = 0; cur_orientation < detect_orientation_side_count; cur_orientation++) { if (!side_data_collected[cur_orientation]) { strncat(pendingStr, " ", sizeof(pendingStr) - 1); strncat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation), sizeof(pendingStr) - 1); } } calibration_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr); usleep(20000); calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side"); usleep(20000); enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel, lenient_still_position); if (orient == DETECT_ORIENTATION_ERROR) { orientation_failures++; calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still..."); usleep(20000); continue; } /* inform user about already handled side */ if (side_data_collected[orient]) { orientation_failures++; set_tune(TONE_NOTIFY_NEGATIVE_TUNE); calibration_log_info(mavlink_log_pub, "[cal] %s side already completed", detect_orientation_str(orient)); usleep(20000); continue; } calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient)); usleep(20000); calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient)); usleep(20000); orientation_failures = 0; // Call worker routine result = calibration_worker(orient, cancel_sub, worker_data); if (result != calibrate_return_ok) { break; } calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient)); usleep(20000); calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient)); usleep(20000); // Note that this side is complete side_data_collected[orient] = true; // output neutral tune set_tune(TONE_NOTIFY_NEUTRAL_TUNE); // temporary priority boost for the white blinking led to come trough rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST, 3, 1); usleep(200000); } if (sub_accel >= 0) { px4_close(sub_accel); } return result; }
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub, bool lenient_still_position) { static constexpr unsigned ndim = 3; float accel_ema[ndim] = { 0.0f }; // exponential moving average of accel float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; // max-hold dispersion of accel static constexpr float ema_len = 0.5f; // EMA time constant in seconds static constexpr float normal_still_thr = 0.25; // normal still threshold float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2); static constexpr float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2 const hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us px4_pollfd_struct_t fds[1]; fds[0].fd = accel_sub; fds[0].events = POLLIN; const hrt_abstime t_start = hrt_absolute_time(); /* set timeout to 30s */ static constexpr hrt_abstime timeout = 90000000; hrt_abstime t_timeout = t_start + timeout; hrt_abstime t = t_start; hrt_abstime t_prev = t_start; hrt_abstime t_still = 0; unsigned poll_errcount = 0; while (true) { /* wait blocking for new data */ int poll_ret = px4_poll(fds, 1, 1000); if (poll_ret) { struct sensor_combined_s sensor; orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor); t = hrt_absolute_time(); float dt = (t - t_prev) / 1000000.0f; t_prev = t; float w = dt / ema_len; for (unsigned i = 0; i < ndim; i++) { float di = sensor.accelerometer_m_s2[i]; float d = di - accel_ema[i]; accel_ema[i] += d * w; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); if (d > still_thr2 * 8.0f) { d = still_thr2 * 8.0f; } if (d > accel_disp[i]) { accel_disp[i] = d; } } /* still detector with hysteresis */ if (accel_disp[0] < still_thr2 && accel_disp[1] < still_thr2 && accel_disp[2] < still_thr2) { /* is still now */ if (t_still == 0) { /* first time */ calibration_log_info(mavlink_log_pub, "[cal] detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; } else { /* still since t_still */ if (t > t_still + still_time) { /* vehicle is still, exit from the loop to detection of its orientation */ break; } } } else if (accel_disp[0] > still_thr2 * 4.0f || accel_disp[1] > still_thr2 * 4.0f || accel_disp[2] > still_thr2 * 4.0f) { /* not still, reset still start time */ if (t_still != 0) { calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still..."); usleep(200000); t_still = 0; } } } else if (poll_ret == 0) { poll_errcount++; } if (t > t_timeout) { poll_errcount++; } if (poll_errcount > 1000) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG); return DETECT_ORIENTATION_ERROR; } } if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && fabsf(accel_ema[2]) < accel_err_thr) { return DETECT_ORIENTATION_TAIL_DOWN; // [ g, 0, 0 ] } if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && fabsf(accel_ema[2]) < accel_err_thr) { return DETECT_ORIENTATION_NOSE_DOWN; // [ -g, 0, 0 ] } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[2]) < accel_err_thr) { return DETECT_ORIENTATION_LEFT; // [ 0, g, 0 ] } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[2]) < accel_err_thr) { return DETECT_ORIENTATION_RIGHT; // [ 0, -g, 0 ] } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) { return DETECT_ORIENTATION_UPSIDE_DOWN; // [ 0, 0, g ] } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) { return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ] } calibration_log_critical(mavlink_log_pub, "ERROR: invalid orientation"); return DETECT_ORIENTATION_ERROR; // Can't detect orientation }