From c3c1e219b87f11d67a20504cbb606597e6a094f8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 29 Sep 2019 14:57:19 -0400 Subject: [PATCH 1/3] commander calibration routines initialize everything --- .../commander/accelerometer_calibration.cpp | 123 +++--- .../commander/calibration_routines.cpp | 390 ++++++------------ src/modules/commander/calibration_routines.h | 36 +- src/modules/commander/gyro_calibration.cpp | 60 ++- src/modules/commander/mag_calibration.cpp | 58 ++- 5 files changed, 235 insertions(+), 432 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ab2696b82d1f..73a6857dba67 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -150,7 +150,8 @@ #include #include -static const char *sensor_name = "accel"; +static constexpr char sensor_name[] = "accel"; +static constexpr unsigned max_accel_sens = 3; static int32_t device_id[max_accel_sens]; static int device_prio_max = 0; @@ -158,20 +159,21 @@ static int32_t device_id_primary = 0; 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 read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); -int mat_invert3(float src[3][3], float dst[3][3]); + calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g); /// Data passed to calibration worker routine typedef struct { - orb_advert_t *mavlink_log_pub; - unsigned done_count; - int subs[max_accel_sens]; - float accel_ref[max_accel_sens][detect_orientation_side_count][3]; - int sensor_correction_sub; + orb_advert_t *mavlink_log_pub{nullptr}; + unsigned done_count{0}; + int subs[max_accel_sens] {-1, -1, -1}; + float accel_ref[max_accel_sens][detect_orientation_side_count][3] {}; + int sensor_correction_sub{-1}; } accel_worker_data_t; int do_accel_calibration(orb_advert_t *mavlink_log_pub) @@ -182,7 +184,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - struct accel_calibration_s accel_scale; + accel_calibration_s accel_scale{}; accel_scale.x_offset = 0.0f; accel_scale.x_scale = 1.0f; accel_scale.y_offset = 0.0f; @@ -192,7 +194,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) int res = PX4_OK; - char str[30]; + char str[30] {}; /* reset all sensors */ for (unsigned s = 0; s < max_accel_sens; s++) { @@ -261,8 +263,8 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) #endif } - float accel_offs[max_accel_sens][3]; - float accel_T[max_accel_sens][3][3]; + float accel_offs[max_accel_sens][3] {}; + float accel_T[max_accel_sens][3][3] {}; unsigned active_sensors = 0; /* measure and calculate offsets & scales */ @@ -291,14 +293,13 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) /* 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; - matrix::Dcmf board_rotation = get_rot_matrix(board_rotation_id); - - matrix::Dcmf board_rotation_t = board_rotation.transpose(); + int32_t board_rotation_int = ROTATION_NONE; + param_get(board_rotation_h, &board_rotation_int); + const enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; + const matrix::Dcmf board_rotation = get_rot_matrix(board_rotation_id); + const matrix::Dcmf board_rotation_t = board_rotation.transpose(); - bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator + bool tc_locked[3] {}; // true when the thermal parameter instance has already been adjusted by the calibrator for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) { @@ -308,6 +309,8 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) matrix::Matrix3f accel_T_mat(accel_T[uorb_index]); matrix::Matrix3f accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation; + accel_scale = accel_calibration_s{}; + 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); @@ -330,8 +333,8 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) (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)); + int32_t tc_enabled_int = 0; + param_get(param_find("TC_A_ENABLE"), &tc_enabled_int); if (tc_enabled_int == 1) { /* Get struct containing sensor thermal compensation data */ @@ -344,13 +347,10 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) 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; + float val = 0.0f; (void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index); - handle = param_find(str); + int32_t handle = param_find(str); param_get(handle, &val); if (axis_index == 0) { @@ -368,9 +368,9 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) /* update the _SCL_ terms to include the scale factor */ for (unsigned axis_index = 0; axis_index < 3; axis_index++) { - val = 1.0f; + float val = 1.0f; (void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index); - handle = param_find(str); + int32_t handle = param_find(str); if (axis_index == 0) { val = accel_scale.x_scale; @@ -499,7 +499,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub worker_data.subs[i] = -1; } - uint64_t timestamps[max_accel_sens] = {}; + 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)); @@ -518,7 +518,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub 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); - sensor_accel_s report = {}; + sensor_accel_s report{}; orb_copy(ORB_ID(sensor_accel), worker_data.subs[cur_accel], &report); #ifdef __PX4_NUTTX @@ -554,7 +554,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub if (device_id[cur_accel] != 0) { // Get priority - int32_t prio; + int32_t prio = ORB_PRIO_DEFAULT; orb_priority(worker_data.subs[cur_accel], &prio); if (prio > device_prio_max) { @@ -620,34 +620,33 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m param_t board_offset_y = param_find("SENS_BOARD_Y_OFF"); param_t board_offset_z = param_find("SENS_BOARD_Z_OFF"); - float board_offset[3]; + float board_offset[3] {}; param_get(board_offset_x, &board_offset[0]); param_get(board_offset_y, &board_offset[1]); param_get(board_offset_z, &board_offset[2]); - matrix::Dcmf board_rotation_offset = matrix::Eulerf( - M_DEG_TO_RAD_F * board_offset[0], - M_DEG_TO_RAD_F * board_offset[1], - M_DEG_TO_RAD_F * board_offset[2]); + const matrix::Dcmf board_rotation_offset = matrix::Eulerf( + math::radians(board_offset[0]), + math::radians(board_offset[1]), + math::radians(board_offset[2])); - int32_t board_rotation_int; - param_get(board_rotation_h, &(board_rotation_int)); + int32_t board_rotation_int = ROTATION_NONE; + param_get(board_rotation_h, &board_rotation_int); - matrix::Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int); + const matrix::Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int); - px4_pollfd_struct_t fds[max_accel_sens]; + px4_pollfd_struct_t fds[max_accel_sens] {}; for (unsigned i = 0; i < max_accel_sens; i++) { fds[i].fd = subs[i]; fds[i].events = POLLIN; } - unsigned counts[max_accel_sens] = { 0 }; - float accel_sum[max_accel_sens][3]; - memset(accel_sum, 0, sizeof(accel_sum)); + unsigned counts[max_accel_sens] {}; + float accel_sum[max_accel_sens][3] {}; unsigned errcount = 0; - struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */ + sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */ /* try to get latest thermal corrections */ if (orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction) != 0) { @@ -672,8 +671,7 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m orb_check(subs[s], &changed); if (changed) { - - sensor_accel_s arp; + sensor_accel_s arp{}; orb_copy(ORB_ID(sensor_accel), subs[s], &arp); // Apply thermal offset corrections in sensor/board frame @@ -731,7 +729,7 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m return calibrate_return_ok; } -int mat_invert3(float src[3][3], float dst[3][3]) +static int mat_invert3(float src[3][3], float dst[3][3]) { float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + @@ -764,8 +762,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, } /* fill matrix A for linear equations system*/ - float mat_A[3][3]; - memset(mat_A, 0, sizeof(mat_A)); + float mat_A[3][3] {}; for (unsigned i = 0; i < 3; i++) { for (unsigned j = 0; j < 3; j++) { @@ -775,7 +772,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, } /* calculate inverse matrix for A */ - float mat_A_inv[3][3]; + float mat_A_inv[3][3] {}; if (mat_invert3(mat_A, mat_A_inv) != PX4_OK) { return calibrate_return_error; @@ -800,8 +797,7 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) bool success = false; int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); + vehicle_attitude_s att{}; calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level"); @@ -810,15 +806,20 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) 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; + float roll_offset_current = 0.0f; param_get(roll_offset_handle, &roll_offset_current); + + float pitch_offset_current = 0.0f; param_get(pitch_offset_handle, &pitch_offset_current); + + int32_t board_rot_current = ROTATION_NONE; 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) { + if ((board_rot_current == 0) + && (fabsf(roll_offset_current) < FLT_EPSILON) + && (fabsf(pitch_offset_current) < FLT_EPSILON)) { + settle_time = 0; } @@ -827,7 +828,7 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) param_set_no_notification(pitch_offset_handle, &zero); param_notify_changes(); - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] {}; fds[0].fd = att_sub; fds[0].events = POLLIN; @@ -884,10 +885,12 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) 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); + float roll_mean_deg = math::degrees(roll_mean); + param_set_no_notification(roll_offset_handle, &roll_mean_deg); + + float pitch_mean_deg = math::degrees(pitch_mean); + param_set_no_notification(pitch_offset_handle, &pitch_mean_deg); + param_notify_changes(); success = true; } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 4ff9a36c1974..3d2106e7d692 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -62,224 +62,29 @@ #include "calibration_messages.h" #include "commander_helper.h" -int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, - float *sphere_radius) -{ - - float x_sumplain = 0.0f; - float x_sumsq = 0.0f; - float x_sumcube = 0.0f; - - float y_sumplain = 0.0f; - float y_sumsq = 0.0f; - float y_sumcube = 0.0f; - - float z_sumplain = 0.0f; - float z_sumsq = 0.0f; - float z_sumcube = 0.0f; - - float xy_sum = 0.0f; - float xz_sum = 0.0f; - float yz_sum = 0.0f; - - float x2y_sum = 0.0f; - float x2z_sum = 0.0f; - float y2x_sum = 0.0f; - float y2z_sum = 0.0f; - float z2x_sum = 0.0f; - float z2y_sum = 0.0f; - - for (unsigned int i = 0; i < size; i++) { - - float x2 = x[i] * x[i]; - float y2 = y[i] * y[i]; - float z2 = z[i] * z[i]; - - x_sumplain += x[i]; - x_sumsq += x2; - x_sumcube += x2 * x[i]; - - y_sumplain += y[i]; - y_sumsq += y2; - y_sumcube += y2 * y[i]; - - z_sumplain += z[i]; - z_sumsq += z2; - z_sumcube += z2 * z[i]; - - xy_sum += x[i] * y[i]; - xz_sum += x[i] * z[i]; - yz_sum += y[i] * z[i]; - - x2y_sum += x2 * y[i]; - x2z_sum += x2 * z[i]; - - y2x_sum += y2 * x[i]; - y2z_sum += y2 * z[i]; - - z2x_sum += z2 * x[i]; - z2y_sum += z2 * y[i]; - } - - // - //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data - // - // P is a structure that has been computed with the data earlier. - // P.npoints is the number of elements; the length of X,Y,Z are identical. - // P's members are logically named. - // - // X[n] is the x component of point n - // Y[n] is the y component of point n - // Z[n] is the z component of point n - // - // A is the x coordiante of the sphere - // B is the y coordiante of the sphere - // C is the z coordiante of the sphere - // Rsq is the radius squared of the sphere. - // - //This method should converge; maybe 5-100 iterations or more. - // - float x_sum = x_sumplain / size; //sum( X[n] ) - float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) - float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) - float y_sum = y_sumplain / size; //sum( Y[n] ) - float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) - float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) - float z_sum = z_sumplain / size; //sum( Z[n] ) - float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) - float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) - - float XY = xy_sum / size; //sum( X[n] * Y[n] ) - float XZ = xz_sum / size; //sum( X[n] * Z[n] ) - float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) - float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) - float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) - float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) - float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) - float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) - float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) - - //Reduction of multiplications - float F0 = x_sum2 + y_sum2 + z_sum2; - float F1 = 0.5f * F0; - float F2 = -8.0f * (x_sum3 + Y2X + Z2X); - float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); - float F4 = -8.0f * (X2Z + Y2Z + z_sum3); - - //Set initial conditions: - float A = x_sum; - float B = y_sum; - float C = z_sum; - - //First iteration computation: - float A2 = A * A; - float B2 = B * B; - float C2 = C * C; - float QS = A2 + B2 + C2; - float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - - //Set initial conditions: - float Rsq = F0 + QB + QS; - - //First iteration computation: - float Q0 = 0.5f * (QS - Rsq); - float Q1 = F1 + Q0; - float Q2 = 8.0f * (QS - Rsq + QB + F0); - float aA, aB, aC, nA, nB, nC, dA, dB, dC; - - //Iterate N times, ignore stop condition. - unsigned int n = 0; - - while (n < max_iterations) { - n++; - - //Compute denominator: - aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); - aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); - aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); - aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA; - aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB; - aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC; - - //Compute next iteration - nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); - nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); - nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); - - //Check for stop condition - dA = (nA - A); - dB = (nB - B); - dC = (nC - C); - - if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } - - //Compute next iteration's values - A = nA; - B = nB; - C = nC; - A2 = A * A; - B2 = B * B; - C2 = C * C; - QS = A2 + B2 + C2; - QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - Rsq = F0 + QB + QS; - Q0 = 0.5f * (QS - Rsq); - Q1 = F1 + Q0; - Q2 = 8.0f * (QS - Rsq + QB + F0); - } - - *sphere_x = A; - *sphere_y = B; - *sphere_z = C; - *sphere_radius = sqrtf(Rsq); - - return 0; -} - -int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, int max_iterations, float delta, 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) -{ - float _fitness = 1.0e30f, _sphere_lambda = 1.0f, _ellipsoid_lambda = 1.0f; - - for (int i = 0; i < max_iterations; i++) { - run_lm_sphere_fit(x, y, z, _fitness, _sphere_lambda, - size, offset_x, offset_y, offset_z, - sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z); - - } - - _fitness = 1.0e30f; - - for (int i = 0; i < max_iterations; i++) { - run_lm_ellipsoid_fit(x, y, z, _fitness, _ellipsoid_lambda, - size, offset_x, offset_y, offset_z, - sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z); - } - - return 0; -} - -int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, - unsigned int size, 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) +static int run_lm_sphere_fit(const float x[], const float y[], const float z[], + float &fitness_in, float &sphere_lambda_in, unsigned int size, + 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) { //Run Sphere Fit using Levenberg Marquardt LSq Fit const float lma_damping = 10.0f; - float _samples_collected = size; - float fitness = _fitness; - float fit1 = 0.0f, fit2 = 0.0f; + float samples_collected = size; + float fitness = fitness_in; + float fit1 = 0.0f; + float fit2 = 0.0f; matrix::SquareMatrix JTJ; matrix::SquareMatrix JTJ2; - float JTFI[4] = {}; + float JTFI[4] {}; float residual = 0.0f; // Gauss Newton Part common for all kind of extensions including LM - for (uint16_t k = 0; k < _samples_collected; k++) { + for (uint16_t k = 0; k < samples_collected; k++) { - float sphere_jacob[4]; + float sphere_jacob[4] {}; //Calculate Jacobian float A = (*diag_x * (x[k] - *offset_x)) + (*offdiag_x * (y[k] - *offset_y)) + (*offdiag_y * (z[k] - *offset_z)); float B = (*offdiag_x * (x[k] - *offset_x)) + (*diag_y * (y[k] - *offset_y)) + (*offdiag_z * (z[k] - *offset_z)); @@ -309,12 +114,12 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & //------------------------Levenberg-Marquardt-part-starts-here---------------------------------// //refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter float fit1_params[4] = {*sphere_radius, *offset_x, *offset_y, *offset_z}; - float fit2_params[4]; + float fit2_params[4] {}; memcpy(fit2_params, fit1_params, sizeof(fit1_params)); for (uint8_t i = 0; i < 4; i++) { - JTJ(i, i) += _sphere_lambda; - JTJ2(i, i) += _sphere_lambda / lma_damping; + JTJ(i, i) += sphere_lambda_in; + JTJ2(i, i) += sphere_lambda_in / lma_damping; } if (!JTJ.I(JTJ)) { @@ -333,7 +138,7 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & } //Calculate mean squared residuals - for (uint16_t k = 0; k < _samples_collected; k++) { + for (uint16_t k = 0; k < samples_collected; k++) { float A = (*diag_x * (x[k] - fit1_params[1])) + (*offdiag_x * (y[k] - fit1_params[2])) + (*offdiag_y * (z[k] + fit1_params[3])); float B = (*offdiag_x * (x[k] - fit1_params[1])) + (*diag_y * (y[k] - fit1_params[2])) + (*offdiag_z * @@ -355,25 +160,25 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & fit2 += residual * residual; } - fit1 = sqrtf(fit1) / _samples_collected; - fit2 = sqrtf(fit2) / _samples_collected; + fit1 = sqrtf(fit1) / samples_collected; + fit2 = sqrtf(fit2) / samples_collected; - if (fit1 > _fitness && fit2 > _fitness) { - _sphere_lambda *= lma_damping; + if (fit1 > fitness_in && fit2 > fitness_in) { + sphere_lambda_in *= lma_damping; - } else if (fit2 < _fitness && fit2 < fit1) { - _sphere_lambda /= lma_damping; + } else if (fit2 < fitness_in && fit2 < fit1) { + sphere_lambda_in /= lma_damping; memcpy(fit1_params, fit2_params, sizeof(fit1_params)); fitness = fit2; - } else if (fit1 < _fitness) { + } else if (fit1 < fitness_in) { fitness = fit1; } //--------------------Levenberg-Marquardt-part-ends-here--------------------------------// - if (PX4_ISFINITE(fitness) && fitness < _fitness) { - _fitness = fitness; + if (PX4_ISFINITE(fitness) && fitness < fitness_in) { + fitness_in = fitness; *sphere_radius = fit1_params[0]; *offset_x = fit1_params[1]; *offset_y = fit1_params[2]; @@ -385,25 +190,26 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & } } -int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, - unsigned int size, 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) +static int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &fitness_in, + float &sphere_lambda_in, + unsigned int size, 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) { //Run Sphere Fit using Levenberg Marquardt LSq Fit const float lma_damping = 10.0f; - float _samples_collected = size; - float fitness = _fitness; + float samples_collected = size; + float fitness = fitness_in; float fit1 = 0.0f; float fit2 = 0.0f; - float JTJ[81] = {}; - float JTJ2[81] = {}; - float JTFI[9] = {}; + float JTJ[81] {}; + float JTJ2[81] {}; + float JTFI[9] {}; float residual = 0.0f; - float ellipsoid_jacob[9]; + float ellipsoid_jacob[9] {}; // Gauss Newton Part common for all kind of extensions including LM - for (uint16_t k = 0; k < _samples_collected; k++) { + for (uint16_t k = 0; k < samples_collected; k++) { //Calculate Jacobian float A = (*diag_x * (x[k] - *offset_x)) + (*offdiag_x * (y[k] - *offset_y)) + (*offdiag_y * (z[k] - *offset_z)); @@ -440,15 +246,14 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa //------------------------Levenberg-Marquardt-part-starts-here---------------------------------// //refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter float fit1_params[9] = {*offset_x, *offset_y, *offset_z, *diag_x, *diag_y, *diag_z, *offdiag_x, *offdiag_y, *offdiag_z}; - float fit2_params[9]; + float fit2_params[9] {}; memcpy(fit2_params, fit1_params, sizeof(fit1_params)); for (uint8_t i = 0; i < 9; i++) { - JTJ[i * 9 + i] += _sphere_lambda; - JTJ2[i * 9 + i] += _sphere_lambda / lma_damping; + JTJ[i * 9 + i] += sphere_lambda_in; + JTJ2[i * 9 + i] += sphere_lambda_in / lma_damping; } - if (!mat_inverse(JTJ, JTJ, 9)) { return -1; } @@ -465,7 +270,7 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa } //Calculate mean squared residuals - for (uint16_t k = 0; k < _samples_collected; k++) { + for (uint16_t k = 0; k < samples_collected; k++) { float A = (fit1_params[3] * (x[k] - fit1_params[0])) + (fit1_params[6] * (y[k] - fit1_params[1])) + (fit1_params[7] * (z[k] - fit1_params[2])); float B = (fit1_params[6] * (x[k] - fit1_params[0])) + (fit1_params[4] * (y[k] - fit1_params[1])) + (fit1_params[8] * @@ -487,24 +292,24 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa fit2 += residual * residual; } - fit1 = sqrtf(fit1) / _samples_collected; - fit2 = sqrtf(fit2) / _samples_collected; + fit1 = sqrtf(fit1) / samples_collected; + fit2 = sqrtf(fit2) / samples_collected; - if (fit1 > _fitness && fit2 > _fitness) { - _sphere_lambda *= lma_damping; + if (fit1 > fitness_in && fit2 > fitness_in) { + sphere_lambda_in *= lma_damping; - } else if (fit2 < _fitness && fit2 < fit1) { - _sphere_lambda /= lma_damping; + } else if (fit2 < fitness_in && fit2 < fit1) { + sphere_lambda_in /= lma_damping; memcpy(fit1_params, fit2_params, sizeof(fit1_params)); fitness = fit2; - } else if (fit1 < _fitness) { + } else if (fit1 < fitness_in) { fitness = fit1; } //--------------------Levenberg-Marquardt-part-ends-here--------------------------------// - if (PX4_ISFINITE(fitness) && fitness < _fitness) { - _fitness = fitness; + if (PX4_ISFINITE(fitness) && fitness < fitness_in) { + fitness_in = fitness; *offset_x = fit1_params[0]; *offset_y = fit1_params[1]; *offset_z = fit1_params[2]; @@ -521,20 +326,51 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa } } -enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub, - bool lenient_still_position) +int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[], + unsigned int size, int max_iterations, float delta, 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) { + float fitness = 1.0e30f; + float sphere_lambda = 1.0f; + float ellipsoid_lambda = 1.0f; + + for (int i = 0; i < max_iterations; i++) { + run_lm_sphere_fit(x, y, z, + fitness, sphere_lambda, size, + offset_x, offset_y, offset_z, + sphere_radius, + diag_x, diag_y, diag_z, + offdiag_x, offdiag_y, offdiag_z); + + } + + fitness = 1.0e30f; + + for (int i = 0; i < max_iterations; i++) { + run_lm_ellipsoid_fit(x, y, z, + fitness, ellipsoid_lambda, size, + offset_x, offset_y, offset_z, + sphere_radius, + diag_x, diag_y, diag_z, + offdiag_x, offdiag_y, offdiag_z); + } + + return 0; +} + +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 + float accel_ema[ndim]{}; // exponential moving average of accel + float accel_disp[3]{}; // 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]; + px4_pollfd_struct_t fds[1]{}; fds[0].fd = accel_sub; fds[0].events = POLLIN; @@ -550,12 +386,13 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, unsigned poll_errcount = 0; - while (true) { + while (true) + { /* wait blocking for new data */ int poll_ret = px4_poll(fds, 1, 1000); if (poll_ret) { - struct sensor_combined_s sensor; + sensor_combined_s sensor{}; orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor); t = hrt_absolute_time(); float dt = (t - t_prev) / 1000000.0f; @@ -584,6 +421,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, 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 */ @@ -602,6 +440,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, } 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..."); @@ -626,37 +465,49 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, 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) { + 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) { + 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) { + 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) { + 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) { + 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) { + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) + { + return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ] } @@ -681,7 +532,7 @@ const char *detect_orientation_str(enum detect_orientation_return orientation) } calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, - int cancel_sub, + int cancel_sub, bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker, void *worker_data, @@ -728,8 +579,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, } /* inform user which orientations are still needed */ - char pendingStr[80]; - pendingStr[0] = 0; + char pendingStr[80] {}; for (unsigned int cur_orientation = 0; cur_orientation < detect_orientation_side_count; cur_orientation++) { if (!side_data_collected[cur_orientation]) { @@ -803,8 +653,8 @@ int calibrate_cancel_subscribe() if (vehicle_command_sub >= 0) { // make sure we won't read any old messages - struct vehicle_command_s cmd; - bool update; + vehicle_command_s cmd{}; + bool update = false; while (orb_check(vehicle_command_sub, &update) == 0 && update) { orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &cmd); @@ -838,12 +688,12 @@ static void calibrate_answer_command(orb_advert_t *mavlink_log_pub, struct vehic bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub) { - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] {}; fds[0].fd = cancel_sub; fds[0].events = POLLIN; if (px4_poll(&fds[0], 1, 0) > 0) { - struct vehicle_command_s cmd; + vehicle_command_s cmd{}; orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd); diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 08c9308f11a5..d8d7c40c1633 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -36,44 +36,10 @@ #pragma once -/** - * Least-squares fit of a sphere to a set of points. - * - * Fits a sphere to a set of points on the sphere surface. - * - * @param x point coordinates on the X axis - * @param y point coordinates on the Y axis - * @param z point coordinates on the Z axis - * @param size number of points - * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100. - * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times. - * @param sphere_x coordinate of the sphere center on the X axis - * @param sphere_y coordinate of the sphere center on the Y axis - * @param sphere_z coordinate of the sphere center on the Z axis - * @param sphere_radius sphere radius - * - * @return 0 on success, 1 on failure - */ -int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, - float *sphere_radius); int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[], unsigned int size, int max_iterations, float delta, 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); -int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, - unsigned int size, 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); -int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, - unsigned int size, 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); -bool inverse4x4(float m[], float invOut[]); -bool mat_inverse(float *A, float *inv, uint8_t n); - -// FIXME: Change the name -static const unsigned max_accel_sens = 3; // The order of these cannot change since the calibration calculations depend on them in this order enum detect_orientation_return { @@ -121,7 +87,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, ///< /// Called at the beginning of calibration in order to subscribe to the cancel command /// @return Handle to vehicle_command subscription -int calibrate_cancel_subscribe(void); +int calibrate_cancel_subscribe(); /// Called to cancel the subscription to the cancel command /// @param cancel_sub Cancel subcription from calibration_cancel_subscribe diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 13ac8308f3a8..e3f337bf8f2f 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -60,18 +60,17 @@ #include #include -static const char *sensor_name = "gyro"; - -static const unsigned max_gyros = 3; +static constexpr char sensor_name[] = "gyro"; +static constexpr unsigned max_gyros = 3; /// Data passed to calibration worker routine typedef struct { - orb_advert_t *mavlink_log_pub; - int32_t device_id[max_gyros]; - int gyro_sensor_sub[max_gyros]; - int sensor_correction_sub; - struct gyro_calibration_s gyro_scale[max_gyros]; - sensor_gyro_s gyro_report_0; + orb_advert_t *mavlink_log_pub{nullptr}; + int32_t device_id[max_gyros] {}; + int gyro_sensor_sub[max_gyros] {-1, -1, -1}; + int sensor_correction_sub{-1}; + gyro_calibration_s gyro_scale[max_gyros] {}; + sensor_gyro_s gyro_report_0{}; } gyro_worker_data_t; static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) @@ -79,10 +78,9 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) gyro_worker_data_t *worker_data = (gyro_worker_data_t *)(data); unsigned calibration_counter[max_gyros] = { 0 }, slow_count = 0; const unsigned calibration_count = 5000; - sensor_gyro_s gyro_report; + sensor_gyro_s gyro_report{}; unsigned poll_errcount = 0; - - struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */ + sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */ if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) { /* use default values */ @@ -95,7 +93,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) } } - px4_pollfd_struct_t fds[max_gyros]; + px4_pollfd_struct_t fds[max_gyros] {}; for (unsigned s = 0; s < max_gyros; s++) { fds[s].fd = worker_data->gyro_sensor_sub[s]; @@ -112,7 +110,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) } /* check if there are new thermal corrections */ - bool updated; + bool updated = false; orb_check(worker_data->sensor_correction_sub, &updated); if (updated) { @@ -130,7 +128,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) continue; } - bool changed; + bool changed = false; orb_check(worker_data->gyro_sensor_sub[s], &changed); if (changed) { @@ -175,14 +173,12 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) } calibration_counter[s]++; - } // Maintain the sample count of the slowest sensor if (calibration_counter[s] && calibration_counter[s] < update_count) { update_count = calibration_counter[s]; } - } if (update_count % (calibration_count / 20) == 0) { @@ -220,19 +216,16 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) int do_gyro_calibration(orb_advert_t *mavlink_log_pub) { - int res = PX4_OK; - gyro_worker_data_t worker_data = {}; + int res = PX4_OK; + gyro_worker_data_t worker_data{}; calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); worker_data.mavlink_log_pub = mavlink_log_pub; - struct gyro_calibration_s gyro_scale_zero; - gyro_scale_zero.x_offset = 0.0f; + gyro_calibration_s gyro_scale_zero{}; gyro_scale_zero.x_scale = 1.0f; - gyro_scale_zero.y_offset = 0.0f; gyro_scale_zero.y_scale = 1.0f; - gyro_scale_zero.z_offset = 0.0f; gyro_scale_zero.z_scale = 1.0f; int device_prio_max = 0; @@ -241,10 +234,11 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); for (unsigned s = 0; s < max_gyros; s++) { - char str[30]; + char str[30] {}; // Reset gyro ids to unavailable. worker_data.device_id[s] = 0; + // And set default subscriber values. worker_data.gyro_sensor_sub[s] = -1; (void)sprintf(str, "CAL_GYRO%u_ID", s); @@ -371,7 +365,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) if (worker_data.device_id[cur_gyro] != 0) { // Get priority - int32_t prio; + int32_t prio = ORB_PRIO_DEFAULT; orb_priority(worker_data.gyro_sensor_sub[cur_gyro], &prio); if (prio > device_prio_max) { @@ -448,20 +442,19 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary))); - bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator + bool tc_locked[3] {}; // true when the thermal parameter instance has already been adjusted by the calibrator for (unsigned uorb_index = 0; uorb_index < max_gyros; uorb_index++) { if (worker_data.device_id[uorb_index] != 0) { - char str[30]; + char str[30] {}; /* check if thermal compensation is enabled */ - int32_t tc_enabled_int; + int32_t tc_enabled_int = 0; param_get(param_find("TC_G_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)); + sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */ orb_copy(ORB_ID(sensor_correction), worker_data.sensor_correction_sub, &sensor_correction); /* don't allow a parameter instance to be calibrated again by another uORB instance */ @@ -469,13 +462,10 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) tc_locked[sensor_correction.gyro_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; + float val = 0.0f; (void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index); - handle = param_find(str); + int32_t handle = param_find(str); param_get(handle, &val); if (axis_index == 0) { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 966bd87bad7d..e74793b1b654 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -62,7 +62,7 @@ #include #include -static const char *sensor_name = "mag"; +static constexpr char sensor_name[] = "mag"; static constexpr unsigned max_mags = 4; static constexpr float mag_sphere_radius = 0.2f; static unsigned int calibration_sides = 6; ///< The total number of sides @@ -82,17 +82,17 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub); /// Data passed to calibration worker routine typedef struct { - orb_advert_t *mavlink_log_pub; - unsigned done_count; - int sub_mag[max_mags]; + orb_advert_t *mavlink_log_pub{nullptr}; + unsigned done_count{0}; + int sub_mag[max_mags] {-1, -1, -1, -1}; unsigned int calibration_points_perside; unsigned int calibration_interval_perside_seconds; uint64_t calibration_interval_perside_useconds; unsigned int calibration_counter_total[max_mags]; - bool side_data_collected[detect_orientation_side_count]; - float *x[max_mags]; - float *y[max_mags]; - float *z[max_mags]; + bool side_data_collected[detect_orientation_side_count] {}; + float *x[max_mags] {}; + float *y[max_mags] {}; + float *z[max_mags] {}; } mag_worker_data_t; @@ -100,7 +100,7 @@ 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; + mag_calibration_s mscale_null{}; mscale_null.x_offset = 0.0f; mscale_null.x_scale = 1.0f; mscale_null.y_offset = 0.0f; @@ -112,7 +112,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) // Determine which mags are available and reset each - char str[30]; + char str[30] {}; // reset the learned EKF mag in-flight bias offsets which have been learned for the previous // sensor calibration and will be invalidated by a new sensor calibration @@ -397,7 +397,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } /* Wait clocking for new data on all gyro */ - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] {}; fds[0].fd = sub_gyro; fds[0].events = POLLIN; size_t fd_count = 1; @@ -438,7 +438,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } // Wait clocking for new data on all mags - px4_pollfd_struct_t fds[max_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++) { @@ -576,7 +576,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside; - char str[30]; + char str[30] {}; // Get actual mag count and alloate only as much memory as needed const unsigned orb_mag_count = orb_group_count(ORB_ID(sensor_mag)); @@ -610,7 +610,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) for (unsigned i = 0; i < orb_mag_count && !found_cur_mag; i++) { worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), i); - struct mag_report report; + sensor_mag_s report{}; orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &report); #ifdef __PX4_NUTTX @@ -676,7 +676,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) } if (result == calibrate_return_ok) { - int cancel_sub = calibrate_cancel_subscribe(); + 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 @@ -696,28 +696,22 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) // 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]; + 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 @@ -803,7 +797,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { - struct mag_calibration_s mscale; + mag_calibration_s mscale{}; mscale.x_scale = 1.0; mscale.y_scale = 1.0; mscale.z_scale = 1.0; From f1d0b5bd87f12b0499451dce330ca6837d1f6dae Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 1 Oct 2019 10:31:16 -0400 Subject: [PATCH 2/3] Update src/modules/commander/calibration_routines.cpp Co-Authored-By: Julian Oes --- src/modules/commander/calibration_routines.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 3d2106e7d692..96b34ca21b14 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -654,7 +654,7 @@ int calibrate_cancel_subscribe() if (vehicle_command_sub >= 0) { // make sure we won't read any old messages vehicle_command_s cmd{}; - bool update = false; + bool updated = false; while (orb_check(vehicle_command_sub, &update) == 0 && update) { orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &cmd); From ce94a5bc3f4445c39009e2e7f6c662c975065ab6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 1 Oct 2019 10:32:11 -0400 Subject: [PATCH 3/3] calibration_routines.cpp fix updated --- src/modules/commander/calibration_routines.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 96b34ca21b14..cf99292e13e8 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -656,7 +656,7 @@ int calibrate_cancel_subscribe() vehicle_command_s cmd{}; bool updated = false; - while (orb_check(vehicle_command_sub, &update) == 0 && update) { + while (orb_check(vehicle_command_sub, &updated) == 0 && updated) { orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &cmd); } }