diff --git a/src/lib/mathlib/math/matrix_alg.cpp b/src/lib/mathlib/math/matrix_alg.cpp index 5e08b80a95dd..35ea91326266 100644 --- a/src/lib/mathlib/math/matrix_alg.cpp +++ b/src/lib/mathlib/math/matrix_alg.cpp @@ -256,135 +256,3 @@ bool mat_inverse(float *A, float *inv, uint8_t n) delete[] L_inv; return ret; } - -bool inverse4x4(float m[], float invOut[]) -{ - float inv[16], det; - uint8_t i; - - inv[0] = m[5] * m[10] * m[15] - - m[5] * m[11] * m[14] - - m[9] * m[6] * m[15] + - m[9] * m[7] * m[14] + - m[13] * m[6] * m[11] - - m[13] * m[7] * m[10]; - - inv[4] = -m[4] * m[10] * m[15] + - m[4] * m[11] * m[14] + - m[8] * m[6] * m[15] - - m[8] * m[7] * m[14] - - m[12] * m[6] * m[11] + - m[12] * m[7] * m[10]; - - inv[8] = m[4] * m[9] * m[15] - - m[4] * m[11] * m[13] - - m[8] * m[5] * m[15] + - m[8] * m[7] * m[13] + - m[12] * m[5] * m[11] - - m[12] * m[7] * m[9]; - - inv[12] = -m[4] * m[9] * m[14] + - m[4] * m[10] * m[13] + - m[8] * m[5] * m[14] - - m[8] * m[6] * m[13] - - m[12] * m[5] * m[10] + - m[12] * m[6] * m[9]; - - inv[1] = -m[1] * m[10] * m[15] + - m[1] * m[11] * m[14] + - m[9] * m[2] * m[15] - - m[9] * m[3] * m[14] - - m[13] * m[2] * m[11] + - m[13] * m[3] * m[10]; - - inv[5] = m[0] * m[10] * m[15] - - m[0] * m[11] * m[14] - - m[8] * m[2] * m[15] + - m[8] * m[3] * m[14] + - m[12] * m[2] * m[11] - - m[12] * m[3] * m[10]; - - inv[9] = -m[0] * m[9] * m[15] + - m[0] * m[11] * m[13] + - m[8] * m[1] * m[15] - - m[8] * m[3] * m[13] - - m[12] * m[1] * m[11] + - m[12] * m[3] * m[9]; - - inv[13] = m[0] * m[9] * m[14] - - m[0] * m[10] * m[13] - - m[8] * m[1] * m[14] + - m[8] * m[2] * m[13] + - m[12] * m[1] * m[10] - - m[12] * m[2] * m[9]; - - inv[2] = m[1] * m[6] * m[15] - - m[1] * m[7] * m[14] - - m[5] * m[2] * m[15] + - m[5] * m[3] * m[14] + - m[13] * m[2] * m[7] - - m[13] * m[3] * m[6]; - - inv[6] = -m[0] * m[6] * m[15] + - m[0] * m[7] * m[14] + - m[4] * m[2] * m[15] - - m[4] * m[3] * m[14] - - m[12] * m[2] * m[7] + - m[12] * m[3] * m[6]; - - inv[10] = m[0] * m[5] * m[15] - - m[0] * m[7] * m[13] - - m[4] * m[1] * m[15] + - m[4] * m[3] * m[13] + - m[12] * m[1] * m[7] - - m[12] * m[3] * m[5]; - - inv[14] = -m[0] * m[5] * m[14] + - m[0] * m[6] * m[13] + - m[4] * m[1] * m[14] - - m[4] * m[2] * m[13] - - m[12] * m[1] * m[6] + - m[12] * m[2] * m[5]; - - inv[3] = -m[1] * m[6] * m[11] + - m[1] * m[7] * m[10] + - m[5] * m[2] * m[11] - - m[5] * m[3] * m[10] - - m[9] * m[2] * m[7] + - m[9] * m[3] * m[6]; - - inv[7] = m[0] * m[6] * m[11] - - m[0] * m[7] * m[10] - - m[4] * m[2] * m[11] + - m[4] * m[3] * m[10] + - m[8] * m[2] * m[7] - - m[8] * m[3] * m[6]; - - inv[11] = -m[0] * m[5] * m[11] + - m[0] * m[7] * m[9] + - m[4] * m[1] * m[11] - - m[4] * m[3] * m[9] - - m[8] * m[1] * m[7] + - m[8] * m[3] * m[5]; - - inv[15] = m[0] * m[5] * m[10] - - m[0] * m[6] * m[9] - - m[4] * m[1] * m[10] + - m[4] * m[2] * m[9] + - m[8] * m[1] * m[6] - - m[8] * m[2] * m[5]; - - det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; - - if (fabsf(det) < 1.1755e-38f) { - return false; - } - - det = 1.0f / det; - - for (i = 0; i < 16; i++) { - invOut[i] = inv[i] * det; - } - - return true; -} diff --git a/src/lib/mathlib/math/matrix_alg.h b/src/lib/mathlib/math/matrix_alg.h index 31a6ccd77d23..8bc58dcb18eb 100644 --- a/src/lib/mathlib/math/matrix_alg.h +++ b/src/lib/mathlib/math/matrix_alg.h @@ -47,4 +47,3 @@ float *mat_mul(float *A, float *B, uint8_t n); bool mat_inverse(float *A, float *inv, uint8_t n); -bool inverse4x4(float m[], float invOut[]); diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 11260ae68c9f..138f6f706846 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -154,7 +154,8 @@ using namespace time_literals; using namespace matrix; -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; @@ -162,20 +163,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) @@ -186,17 +188,14 @@ 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_scale.x_offset = 0.0f; + accel_calibration_s accel_scale{}; 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]; + char str[30] {}; /* reset all sensors */ for (unsigned s = 0; s < max_accel_sens; s++) { @@ -265,8 +264,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 */ @@ -295,14 +294,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; - Dcmf board_rotation = get_rot_matrix(board_rotation_id); - - 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 Dcmf board_rotation = get_rot_matrix(board_rotation_id); + const 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++) { @@ -334,8 +332,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 */ @@ -348,13 +346,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); + param_t handle = param_find(str); param_get(handle, &val); if (axis_index == 0) { @@ -372,9 +367,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); + param_t handle = param_find(str); if (axis_index == 0) { val = accel_scale.x_scale; @@ -503,7 +498,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)); @@ -522,7 +517,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 @@ -558,7 +553,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) { @@ -624,33 +619,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]); - Dcmf board_rotation_offset = 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 Dcmf board_rotation_offset = 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); - Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int); + const 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 }; + 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) { @@ -676,7 +671,7 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m 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 @@ -734,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]) + @@ -767,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++) { @@ -778,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; @@ -808,9 +802,9 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) param_t board_rot_handle = param_find("SENS_BOARD_ROT"); // get old values - float roll_offset_current; - float pitch_offset_current; - int32_t board_rot_current = 0; + float roll_offset_current = 0.0f; + float pitch_offset_current = 0.0f; + int32_t board_rot_current = ROTATION_NONE; param_get(roll_offset_handle, &roll_offset_current); param_get(pitch_offset_handle, &pitch_offset_current); param_get(board_rot_handle, &board_rot_current); @@ -820,7 +814,7 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) math::radians(pitch_offset_current), 0.f); - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] {}; fds[0].fd = att_sub; fds[0].events = POLLIN; diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index cafe5e84e8ef..3ea3322b300f 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -62,200 +62,27 @@ #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 *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 sphere_fit_only) { - float _fitness = 1.0e30f, _sphere_lambda = 1.0f, _ellipsoid_lambda = 1.0f; + 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, + 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); } if (!sphere_fit_only) { - _fitness = 1.0e30f; + fitness = 1.0e30f; for (int i = 0; i < max_iterations; i++) { - run_lm_ellipsoid_fit(x, y, z, _fitness, _ellipsoid_lambda, + 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); } @@ -264,25 +91,26 @@ int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[ return 0; } -int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, +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)); @@ -312,12 +140,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)) { @@ -336,7 +164,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 * @@ -358,25 +186,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]; @@ -388,25 +216,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, +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)); @@ -443,15 +272,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; } @@ -468,7 +296,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] * @@ -490,24 +318,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]; @@ -527,17 +355,17 @@ 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) { - 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]; + static constexpr unsigned ndim = 3; + 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] {}; fds[0].fd = accel_sub; fds[0].events = POLLIN; @@ -558,7 +386,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, 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; @@ -587,6 +415,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 */ @@ -605,6 +434,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..."); @@ -684,7 +514,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, @@ -806,10 +636,10 @@ 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 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); } } @@ -841,12 +671,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 ff94941eeb47..11b3ed315e64 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -36,44 +36,22 @@ #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); +#include + int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[], unsigned int size, int max_iterations, 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 sphere_fit_only); + 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 { @@ -85,7 +63,7 @@ enum detect_orientation_return { DETECT_ORIENTATION_RIGHTSIDE_UP, DETECT_ORIENTATION_ERROR }; -static const unsigned detect_orientation_side_count = 6; +static constexpr unsigned detect_orientation_side_count = 6; /// Wait for vehicle to become still and detect it's orientation /// @return Returns detect_orientation_return according to orientation when vehicle @@ -121,7 +99,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 36c5e4b1c45c..50972b4e5d9f 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -61,18 +61,17 @@ #include #include -static const char *sensor_name = "gyro"; - +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]; - float last_sample_0[3]; + 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] {}; + float last_sample_0[3] {}; } gyro_worker_data_t; static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) @@ -80,10 +79,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 = 250; - 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) { for (unsigned i = 0; i < 3; i++) { @@ -93,7 +91,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]; @@ -109,7 +107,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) { @@ -127,7 +125,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) { @@ -165,14 +163,12 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) worker_data->gyro_scale[s].y_offset += sample[1]; worker_data->gyro_scale[s].z_offset += sample[2]; 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) { @@ -210,8 +206,8 @@ 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); @@ -224,10 +220,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); @@ -333,7 +330,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) { @@ -410,20 +407,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 */ @@ -431,13 +427,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); + param_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 bb2781db8dd6..962a0bbca4c7 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, int32_t cal_ma /// 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,19 +100,16 @@ 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; + mag_calibration_s mscale_null{}; 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 = PX4_OK; // 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 @@ -398,7 +395,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; @@ -439,7 +436,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++) { @@ -572,7 +569,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma 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)); @@ -606,7 +603,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma 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 @@ -672,7 +669,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma } 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 @@ -692,28 +689,22 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma // 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 @@ -802,7 +793,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { - mag_calibration_s mscale; + mag_calibration_s mscale{}; + mscale.x_scale = 1.0; + mscale.y_scale = 1.0; + mscale.z_scale = 1.0; #ifdef __PX4_NUTTX int fd_mag = -1;