diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 4750ddbd56..166c0b9aa0 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -263,7 +263,7 @@ int do_accel_calibration(int mavlink_fd) 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))); @@ -285,7 +285,7 @@ int do_accel_calibration(int mavlink_fd) (void)sprintf(str, "CAL_ACC%u_ID", i); failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i]))); #endif - + if (failed) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i); return ERROR; @@ -325,7 +325,7 @@ int do_accel_calibration(int mavlink_fd) } else { mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } - + /* give this message enough time to propagate */ usleep(600000); @@ -336,30 +336,30 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien { const unsigned samples_num = 750; accel_worker_data_t* worker_data = (accel_worker_data_t*)(data); - + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation)); - + read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num); - + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation), (double)worker_data->accel_ref[0][orientation][0], (double)worker_data->accel_ref[0][orientation][1], (double)worker_data->accel_ref[0][orientation][2]); - + worker_data->done_count++; mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count); - + return calibrate_return_ok; } calibrate_return do_accel_calibration_measurements(int mavlink_fd, 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_fd = mavlink_fd; worker_data.done_count = 0; @@ -378,7 +378,7 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel result = calibrate_return_error; break; } - + /* 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); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index f027ed0155..114a4c527d 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -84,58 +84,58 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) const unsigned calibration_count = 5000; struct gyro_report gyro_report; unsigned poll_errcount = 0; - + px4_pollfd_struct_t fds[max_gyros]; for (unsigned s = 0; s < max_gyros; s++) { fds[s].fd = worker_data->gyro_sensor_sub[s]; fds[s].events = POLLIN; } - + memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0)); memset(&worker_data->gyro_scale, 0, sizeof(worker_data->gyro_scale)); - + /* use first gyro to pace, but count correctly per-gyro for statistics */ while (calibration_counter[0] < calibration_count) { if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { return calibrate_return_cancelled; } - + int poll_ret = px4_poll(&fds[0], max_gyros, 1000); - + if (poll_ret > 0) { - + for (unsigned s = 0; s < max_gyros; s++) { bool changed; orb_check(worker_data->gyro_sensor_sub[s], &changed); - + if (changed) { orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report); - + if (s == 0) { orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0); } - + worker_data->gyro_scale[s].x_offset += gyro_report.x; worker_data->gyro_scale[s].y_offset += gyro_report.y; worker_data->gyro_scale[s].z_offset += gyro_report.z; calibration_counter[s]++; } - + if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); } } - + } else { poll_errcount++; } - + if (poll_errcount > 1000) { mavlink_and_console_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); return calibrate_return_error; } } - + for (unsigned s = 0; s < max_gyros; s++) { if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) { mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) @@ -158,7 +158,7 @@ int do_gyro_calibration(int mavlink_fd) mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); worker_data.mavlink_fd = mavlink_fd; - + struct gyro_scale gyro_scale_zero = { 0.0f, // x offset 1.0f, // x scale @@ -167,7 +167,7 @@ int do_gyro_calibration(int mavlink_fd) 0.0f, // z offset 1.0f, // z scale }; - + int device_prio_max = 0; int32_t device_id_primary = 0; @@ -190,7 +190,7 @@ int do_gyro_calibration(int mavlink_fd) return ERROR; } #endif - + // Reset all offsets to 0 and scales to 1 (void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale)); #ifndef __PX4_QURT @@ -207,9 +207,9 @@ int do_gyro_calibration(int mavlink_fd) } } #endif - + } - + for (unsigned s = 0; s < max_gyros; s++) { worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); @@ -228,7 +228,7 @@ int do_gyro_calibration(int mavlink_fd) unsigned try_count = 0; unsigned max_tries = 20; res = ERROR; - + do { // Calibrate gyro and ensure user didn't move calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data); @@ -274,7 +274,7 @@ int do_gyro_calibration(int mavlink_fd) } calibrate_cancel_unsubscribe(cancel_sub); - + for (unsigned s = 0; s < max_gyros; s++) { px4_close(worker_data.gyro_sensor_sub[s]); } @@ -289,7 +289,7 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { if (worker_data.device_id[s] != 0) { char str[30]; - + (void)sprintf(str, "CAL_GYRO%u_XOFF", s); failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].x_offset))); (void)sprintf(str, "CAL_GYRO%u_YOFF", s); @@ -353,6 +353,6 @@ int do_gyro_calibration(int mavlink_fd) /* give this message enough time to propagate */ usleep(600000); - + return res; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index ef9c536b5a..351fd2dd73 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -114,7 +114,7 @@ int do_mag_calibration(int mavlink_fd) #endif int result = OK; - + // Determine which mags are available and reset each char str[30]; @@ -122,7 +122,7 @@ int do_mag_calibration(int mavlink_fd) for (size_t i=0; imavlink_fd, "[cal] Rotate vehicle around the detected orientation"); mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); @@ -309,20 +309,20 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } 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_fd, 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; @@ -334,7 +334,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } } int poll_ret = px4_poll(fds, fd_count, 1000); - + if (poll_ret > 0) { int prev_count[max_mags]; @@ -354,7 +354,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta 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; @@ -380,21 +380,21 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } else { poll_errcount++; } - + if (poll_errcount > worker_data->calibration_points_perside * 3) { result = calibrate_return_error; mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); break; } } - + if (result == calibrate_return_ok) { mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation)); - + worker_data->done_count++; mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data)); } - + return result; } @@ -403,7 +403,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag calibrate_return result = calibrate_return_ok; mag_worker_data_t worker_data; - + worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; worker_data.calibration_points_perside = calibration_total_points / calibration_sides; @@ -417,11 +417,11 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false; worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false; worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false; - + for (size_t cur_mag=0; cur_mag(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.y[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); @@ -443,7 +443,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag } } - + // Setup subscriptions to mag sensors if (result == calibrate_return_ok) { for (unsigned cur_mag=0; 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>>>>>>\n"); } } - + // Data points are no longer needed for (size_t cur_mag=0; cur_mag= 0) { px4_close(fd_mag); @@ -631,7 +631,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag if (result == calibrate_return_ok) { bool failed = false; - + /* set parameters */ #ifndef __PX4_QURT