forked from Archive/PX4-Autopilot
commander: fix code style
This commit is contained in:
Siddharth Bharat Purohit
Lorenz Meier
parent
f811777789
commit
c9ac15f0dd
|
|
@ -238,12 +238,14 @@ int ellipsoid_fit_least_squares(const float x[], const float y[], const float 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;
|
||||
for(int i=0; i < max_iterations; i++) {
|
||||
|
||||
for (int i = 0; i < max_iterations; i++) {
|
||||
//printf("%d, offset: %.6f %.6f %.6f %.6f fitness: %.6f\n", i, (double)*offset_x, (double)*offset_y, (double)*offset_z, (double)*sphere_radius, (double)_fitness);
|
||||
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);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -261,33 +263,35 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
|
|||
float JTJ2[16];
|
||||
float JTFI[4];
|
||||
float residual = 0.0f;
|
||||
memset(&JTJ,0,sizeof(JTJ));
|
||||
memset(&JTJ2,0,sizeof(JTJ2));
|
||||
memset(&JTFI,0,sizeof(JTFI));
|
||||
memset(&JTJ, 0, sizeof(JTJ));
|
||||
memset(&JTJ2, 0, sizeof(JTJ2));
|
||||
memset(&JTFI, 0, sizeof(JTFI));
|
||||
|
||||
// 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];
|
||||
//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));
|
||||
float C = (*offdiag_y * (x[k] + *offset_x)) + (*offdiag_z * (y[k] + *offset_y)) + (*diag_z * (z[k] + *offset_z));
|
||||
float length = sqrtf(A*A + B*B + C*C);
|
||||
float length = sqrtf(A * A + B * B + C * C);
|
||||
|
||||
// 0: partial derivative (radius wrt fitness fn) fn operated on sample
|
||||
sphere_jacob[0] = 1.0f;
|
||||
// 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample
|
||||
sphere_jacob[1] = -1.0f * (((*diag_x * A) + (*offdiag_x * B) + (*offdiag_y * C))/length);
|
||||
sphere_jacob[2] = -1.0f * (((*offdiag_x * A) + (*diag_y * B) + (*offdiag_z * C))/length);
|
||||
sphere_jacob[3] = -1.0f * (((*offdiag_y * A) + (*offdiag_z * B) + (*diag_z * C))/length);
|
||||
sphere_jacob[1] = -1.0f * (((*diag_x * A) + (*offdiag_x * B) + (*offdiag_y * C)) / length);
|
||||
sphere_jacob[2] = -1.0f * (((*offdiag_x * A) + (*diag_y * B) + (*offdiag_z * C)) / length);
|
||||
sphere_jacob[3] = -1.0f * (((*offdiag_y * A) + (*offdiag_z * B) + (*diag_z * C)) / length);
|
||||
residual = *sphere_radius - length;
|
||||
|
||||
for(uint8_t i = 0;i < 4; i++) {
|
||||
for (uint8_t i = 0; i < 4; i++) {
|
||||
// compute JTJ
|
||||
for(uint8_t j = 0; j < 4; j++) {
|
||||
JTJ[i*4 + j] += sphere_jacob[i] * sphere_jacob[j];
|
||||
JTJ2[i*4 + j] += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
|
||||
for (uint8_t j = 0; j < 4; j++) {
|
||||
JTJ[i * 4 + j] += sphere_jacob[i] * sphere_jacob[j];
|
||||
JTJ2[i * 4 + j] += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
|
||||
}
|
||||
|
||||
JTFI[i] += sphere_jacob[i] * residual;
|
||||
}
|
||||
}
|
||||
|
|
@ -297,72 +301,83 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
|
|||
//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];
|
||||
memcpy(fit2_params,fit1_params,sizeof(fit1_params));
|
||||
memcpy(fit2_params, fit1_params, sizeof(fit1_params));
|
||||
|
||||
for(uint8_t i = 0; i < 4; i++) {
|
||||
JTJ[i*4 + i] += _sphere_lambda;
|
||||
JTJ2[i*4 + i] += _sphere_lambda/lma_damping;
|
||||
for (uint8_t i = 0; i < 4; i++) {
|
||||
JTJ[i * 4 + i] += _sphere_lambda;
|
||||
JTJ2[i * 4 + i] += _sphere_lambda / lma_damping;
|
||||
}
|
||||
|
||||
if(!inverse4x4(JTJ, JTJ)) {
|
||||
if (!inverse4x4(JTJ, JTJ)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if(!inverse4x4(JTJ2, JTJ2)) {
|
||||
if (!inverse4x4(JTJ2, JTJ2)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
for(uint8_t row=0; row < 4; row++) {
|
||||
for(uint8_t col=0; col < 4; col++) {
|
||||
fit1_params[row] -= JTFI[col] * JTJ[row*4 + col];
|
||||
fit2_params[row] -= JTFI[col] * JTJ2[row*4 + col];
|
||||
for (uint8_t row = 0; row < 4; row++) {
|
||||
for (uint8_t col = 0; col < 4; col++) {
|
||||
fit1_params[row] -= JTFI[col] * JTJ[row * 4 + col];
|
||||
fit2_params[row] -= JTFI[col] * JTJ2[row * 4 + col];
|
||||
}
|
||||
}
|
||||
|
||||
//Calculate mean squared residuals
|
||||
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 * (z[k] + fit1_params[3]));
|
||||
float C = (*offdiag_y * (x[k] + fit1_params[1])) + (*offdiag_z * (y[k] + fit1_params[2])) + (*diag_z * (z[k] + fit1_params[3]));
|
||||
float length = sqrtf(A*A + B*B + C*C);
|
||||
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 *
|
||||
(z[k] + fit1_params[3]));
|
||||
float C = (*offdiag_y * (x[k] + fit1_params[1])) + (*offdiag_z * (y[k] + fit1_params[2])) + (*diag_z *
|
||||
(z[k] + fit1_params[3]));
|
||||
float length = sqrtf(A * A + B * B + C * C);
|
||||
residual = fit1_params[0] - length;
|
||||
fit1 += residual*residual;
|
||||
fit1 += residual * residual;
|
||||
|
||||
A = (*diag_x * (x[k] + fit2_params[1])) + (*offdiag_x * (y[k] + fit2_params[2])) + (*offdiag_y * (z[k] + fit2_params[3]));
|
||||
B = (*offdiag_x * (x[k] + fit2_params[1])) + (*diag_y * (y[k] + fit2_params[2])) + (*offdiag_z * (z[k] + fit2_params[3]));
|
||||
C = (*offdiag_y * (x[k] + fit2_params[1])) + (*offdiag_z * (y[k] + fit2_params[2])) + (*diag_z * (z[k] + fit2_params[3]));
|
||||
length = sqrtf(A*A + B*B + C*C);
|
||||
A = (*diag_x * (x[k] + fit2_params[1])) + (*offdiag_x * (y[k] + fit2_params[2])) + (*offdiag_y *
|
||||
(z[k] + fit2_params[3]));
|
||||
B = (*offdiag_x * (x[k] + fit2_params[1])) + (*diag_y * (y[k] + fit2_params[2])) + (*offdiag_z *
|
||||
(z[k] + fit2_params[3]));
|
||||
C = (*offdiag_y * (x[k] + fit2_params[1])) + (*offdiag_z * (y[k] + fit2_params[2])) + (*diag_z *
|
||||
(z[k] + fit2_params[3]));
|
||||
length = sqrtf(A * A + B * B + C * C);
|
||||
residual = fit2_params[0] - length;
|
||||
fit2 += residual*residual;
|
||||
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){
|
||||
if (fit1 > _fitness && fit2 > _fitness) {
|
||||
_sphere_lambda *= lma_damping;
|
||||
} else if(fit2 < _fitness && fit2 < fit1) {
|
||||
|
||||
} else if (fit2 < _fitness && fit2 < fit1) {
|
||||
_sphere_lambda /= lma_damping;
|
||||
memcpy(fit1_params,fit2_params,sizeof(fit1_params));
|
||||
memcpy(fit1_params, fit2_params, sizeof(fit1_params));
|
||||
fitness = fit2;
|
||||
} else if(fit1 < _fitness){
|
||||
|
||||
} else if (fit1 < _fitness) {
|
||||
fitness = fit1;
|
||||
}
|
||||
|
||||
//--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
|
||||
|
||||
if(!isnan(fitness) && fitness < _fitness) {
|
||||
if (!isnan(fitness) && fitness < _fitness) {
|
||||
_fitness = fitness;
|
||||
*sphere_radius = fit1_params[0];
|
||||
*offset_x = fit1_params[1];
|
||||
*offset_y = fit1_params[2];
|
||||
*offset_z = fit1_params[3];
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool inverse4x4(float m[],float invOut[])
|
||||
bool inverse4x4(float m[], float invOut[])
|
||||
{
|
||||
float inv[16], det;
|
||||
uint8_t i;
|
||||
|
|
@ -481,18 +496,21 @@ bool inverse4x4(float m[],float invOut[])
|
|||
|
||||
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
|
||||
|
||||
if(fabsf(det) < 1.1755e-38f){
|
||||
if (fabsf(det) < 1.1755e-38f) {
|
||||
return false;
|
||||
}
|
||||
|
||||
det = 1.0f / det;
|
||||
|
||||
for (i = 0; i < 16; i++)
|
||||
for (i = 0; i < 16; i++) {
|
||||
invOut[i] = inv[i] * det;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub, bool lenient_still_position)
|
||||
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub,
|
||||
bool lenient_still_position)
|
||||
{
|
||||
const unsigned ndim = 3;
|
||||
|
||||
|
|
@ -633,9 +651,9 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
|
|||
return DETECT_ORIENTATION_ERROR; // Can't detect orientation
|
||||
}
|
||||
|
||||
const char* detect_orientation_str(enum detect_orientation_return orientation)
|
||||
const char *detect_orientation_str(enum detect_orientation_return orientation)
|
||||
{
|
||||
static const char* rgOrientationStrs[] = {
|
||||
static const char *rgOrientationStrs[] = {
|
||||
"back", // tail down
|
||||
"front", // nose down
|
||||
"left",
|
||||
|
|
@ -652,7 +670,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
|
|||
int cancel_sub,
|
||||
bool side_data_collected[detect_orientation_side_count],
|
||||
calibration_from_orientation_worker_t calibration_worker,
|
||||
void* worker_data,
|
||||
void *worker_data,
|
||||
bool lenient_still_position)
|
||||
{
|
||||
calibrate_return result = calibrate_return_ok;
|
||||
|
|
@ -660,6 +678,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
|
|||
// Setup subscriptions to onboard accel sensor
|
||||
|
||||
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
|
||||
|
||||
if (sub_accel < 0) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
|
||||
return calibrate_return_error;
|
||||
|
|
@ -698,17 +717,19 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
|
|||
char pendingStr[80];
|
||||
pendingStr[0] = 0;
|
||||
|
||||
for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) {
|
||||
for (unsigned int cur_orientation = 0; cur_orientation < detect_orientation_side_count; cur_orientation++) {
|
||||
if (!side_data_collected[cur_orientation]) {
|
||||
strncat(pendingStr, " ", sizeof(pendingStr) - 1);
|
||||
strncat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation), sizeof(pendingStr) - 1);
|
||||
}
|
||||
}
|
||||
|
||||
calibration_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
|
||||
usleep(20000);
|
||||
calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
|
||||
usleep(20000);
|
||||
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel, lenient_still_position);
|
||||
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel,
|
||||
lenient_still_position);
|
||||
|
||||
if (orient == DETECT_ORIENTATION_ERROR) {
|
||||
orientation_failures++;
|
||||
|
|
@ -733,7 +754,8 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
|
|||
|
||||
// Call worker routine
|
||||
result = calibration_worker(orient, cancel_sub, worker_data);
|
||||
if (result != calibrate_return_ok ) {
|
||||
|
||||
if (result != calibrate_return_ok) {
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -804,6 +826,7 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
|
|||
calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG);
|
||||
return true;
|
||||
|
||||
} else {
|
||||
calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -57,11 +57,13 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
|||
float *sphere_radius);
|
||||
int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, unsigned 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 *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);
|
||||
bool inverse4x4(float m[],float invOut[]);
|
||||
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[]);
|
||||
// FIXME: Change the name
|
||||
static const unsigned max_accel_sens = 3;
|
||||
|
||||
|
|
@ -87,7 +89,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
|
|||
|
||||
/// Returns the human readable string representation of the orientation
|
||||
/// @param orientation Orientation to return string for, "error" if buffer is too small
|
||||
const char* detect_orientation_str(enum detect_orientation_return orientation);
|
||||
const char *detect_orientation_str(enum detect_orientation_return orientation);
|
||||
|
||||
enum calibrate_return {
|
||||
calibrate_return_ok,
|
||||
|
|
@ -95,9 +97,10 @@ enum calibrate_return {
|
|||
calibrate_return_cancelled
|
||||
};
|
||||
|
||||
typedef calibrate_return (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, ///< Orientation which was detected
|
||||
typedef calibrate_return(*calibration_from_orientation_worker_t)(detect_orientation_return
|
||||
orientation, ///< Orientation which was detected
|
||||
int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
|
||||
void* worker_data); ///< Opaque worker data
|
||||
void *worker_data); ///< Opaque worker data
|
||||
|
||||
/// Perform calibration sequence which require a rest orientation detection prior to calibration.
|
||||
/// @return OK: Calibration succeeded, ERROR: Calibration failed
|
||||
|
|
@ -105,7 +108,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, ///<
|
|||
int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
|
||||
bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration
|
||||
calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration
|
||||
void* worker_data, ///< Opaque data passed to worker routine
|
||||
void *worker_data, ///< Opaque data passed to worker routine
|
||||
bool lenient_still_detection); ///< true: Use more lenient still position detection
|
||||
|
||||
/// Called at the beginning of calibration in order to subscribe to the cancel command
|
||||
|
|
|
|||
|
|
@ -68,7 +68,8 @@ static unsigned int calibration_sides = 6; ///< The total number of sides
|
|||
static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer
|
||||
static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take
|
||||
|
||||
static constexpr float MAG_MAX_OFFSET_LEN = 1.3f; ///< The maximum measurement range is ~1.9 Ga, the earth field is ~0.6 Ga, so an offset larger than ~1.3 Ga means the mag will saturate in some directions.
|
||||
static constexpr float MAG_MAX_OFFSET_LEN =
|
||||
1.3f; ///< The maximum measurement range is ~1.9 Ga, the earth field is ~0.6 Ga, so an offset larger than ~1.3 Ga means the mag will saturate in some directions.
|
||||
|
||||
int32_t device_ids[max_mags];
|
||||
bool internal[max_mags];
|
||||
|
|
@ -112,7 +113,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
|
|||
|
||||
char str[30];
|
||||
|
||||
for (size_t i=0; i < max_mags; i++) {
|
||||
for (size_t i = 0; i < max_mags; i++) {
|
||||
device_ids[i] = 0; // signals no mag
|
||||
}
|
||||
|
||||
|
|
@ -123,48 +124,63 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
|
|||
// Reset mag id to mag not available
|
||||
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
|
||||
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
|
||||
|
||||
if (result != PX4_OK) {
|
||||
calibration_log_info(mavlink_log_pub, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
|
||||
break;
|
||||
}
|
||||
|
||||
#else
|
||||
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.x_offset);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.y_offset);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.z_offset);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.x_scale);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.y_scale);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.z_scale);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* for calibration, commander will run on apps, so orb messages are used to get info from dsp */
|
||||
/* for calibration, commander will run on apps, so orb messages are used to get info from dsp */
|
||||
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
|
||||
// Attempt to open mag
|
||||
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
|
||||
int fd = px4_open(str, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
continue;
|
||||
}
|
||||
|
|
@ -216,10 +232,12 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
|
|||
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
|
||||
usleep(20000);
|
||||
break;
|
||||
|
||||
} else {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
|
||||
usleep(20000);
|
||||
}
|
||||
|
||||
// Fall through
|
||||
|
||||
default:
|
||||
|
|
@ -235,7 +253,8 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
|
|||
return result;
|
||||
}
|
||||
|
||||
static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count)
|
||||
static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count,
|
||||
unsigned max_count)
|
||||
{
|
||||
float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
|
||||
|
||||
|
|
@ -253,20 +272,22 @@ static bool reject_sample(float sx, float sy, float sz, float x[], float y[], fl
|
|||
return false;
|
||||
}
|
||||
|
||||
static unsigned progress_percentage(mag_worker_data_t* worker_data) {
|
||||
static unsigned progress_percentage(mag_worker_data_t *worker_data)
|
||||
{
|
||||
return 100 * ((float)worker_data->done_count) / calibration_sides;
|
||||
}
|
||||
|
||||
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
|
||||
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void *data)
|
||||
{
|
||||
calibrate_return result = calibrate_return_ok;
|
||||
|
||||
unsigned int calibration_counter_side;
|
||||
|
||||
mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
|
||||
mag_worker_data_t *worker_data = (mag_worker_data_t *)(data);
|
||||
|
||||
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Rotate vehicle around the detected orientation");
|
||||
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Continue rotation for %s %u s", detect_orientation_str(orientation), worker_data->calibration_interval_perside_seconds);
|
||||
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Continue rotation for %s %u s",
|
||||
detect_orientation_str(orientation), worker_data->calibration_interval_perside_seconds);
|
||||
|
||||
/*
|
||||
* Detect if the system is rotating.
|
||||
|
|
@ -349,7 +370,8 @@ 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];
|
||||
size_t fd_count = 0;
|
||||
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
|
||||
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||
if (worker_data->sub_mag[cur_mag] >= 0 && device_ids[cur_mag] != 0) {
|
||||
fds[fd_count].fd = worker_data->sub_mag[cur_mag];
|
||||
fds[fd_count].events = POLLIN;
|
||||
|
|
@ -364,7 +386,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
|||
int prev_count[max_mags];
|
||||
bool rejected = false;
|
||||
|
||||
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||
|
||||
prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag];
|
||||
|
||||
|
|
@ -392,11 +414,13 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
|||
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||
worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
|
||||
}
|
||||
|
||||
} else {
|
||||
calibration_counter_side++;
|
||||
|
||||
unsigned new_progress = progress_percentage(worker_data) +
|
||||
(unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside));
|
||||
(unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)
|
||||
worker_data->calibration_points_perside));
|
||||
|
||||
if (new_progress - _last_mag_progress > 3) {
|
||||
// Progress indicator for side
|
||||
|
|
@ -408,6 +432,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
|||
_last_mag_progress = new_progress;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
|
@ -420,7 +445,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
|||
}
|
||||
|
||||
if (result == calibrate_return_ok) {
|
||||
calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
|
||||
calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side done, rotate to a different side",
|
||||
detect_orientation_str(orientation));
|
||||
|
||||
worker_data->done_count++;
|
||||
usleep(20000);
|
||||
|
|
@ -456,6 +482,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
// mark as missing
|
||||
worker_data.side_data_collected[i] = false;
|
||||
calibration_sides++;
|
||||
|
||||
} else {
|
||||
// mark as completed from the beginning
|
||||
worker_data.side_data_collected[i] = true;
|
||||
|
|
@ -467,7 +494,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
}
|
||||
}
|
||||
|
||||
for (size_t cur_mag = 0; cur_mag<max_mags; cur_mag++) {
|
||||
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||
// Initialize to no subscription
|
||||
worker_data.sub_mag[cur_mag] = -1;
|
||||
|
||||
|
|
@ -482,10 +509,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
|
||||
char str[30];
|
||||
|
||||
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||
worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
|
||||
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
|
||||
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
|
||||
|
||||
if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory");
|
||||
result = calibrate_return_error;
|
||||
|
|
@ -509,6 +537,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
|
||||
device_ids[cur_mag] = mag_report.device_id;
|
||||
#endif
|
||||
|
||||
if (worker_data.sub_mag[cur_mag] < 0) {
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag);
|
||||
result = calibrate_return_error;
|
||||
|
|
@ -524,6 +553,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
device_prio_max = prio;
|
||||
device_id_primary = device_ids[cur_mag];
|
||||
}
|
||||
|
||||
} else {
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag);
|
||||
result = calibrate_return_error;
|
||||
|
|
@ -534,10 +564,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
|
||||
// Limit update rate to get equally spaced measurements over time (in ms)
|
||||
if (result == calibrate_return_ok) {
|
||||
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||
if (device_ids[cur_mag] != 0) {
|
||||
// Mag in this slot is available
|
||||
unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
|
||||
unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) /
|
||||
worker_data.calibration_points_perside;
|
||||
|
||||
//calibration_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs);
|
||||
orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
|
||||
|
|
@ -559,7 +590,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
}
|
||||
|
||||
// Close subscriptions
|
||||
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||
if (worker_data.sub_mag[cur_mag] >= 0) {
|
||||
px4_close(worker_data.sub_mag[cur_mag]);
|
||||
}
|
||||
|
|
@ -581,7 +612,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
|
||||
// Sphere fit the data to get calibration values
|
||||
if (result == calibrate_return_ok) {
|
||||
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||
if (device_ids[cur_mag] != 0) {
|
||||
// Mag in this slot is available and we should have values for it to calibrate
|
||||
|
||||
|
|
@ -589,7 +620,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
worker_data.calibration_counter_total[cur_mag],
|
||||
100, 0.0f,
|
||||
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
|
||||
&sphere_radius[cur_mag], &diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag], &offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]);
|
||||
&sphere_radius[cur_mag], &diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag], &offdiag_x[cur_mag], &offdiag_y[cur_mag],
|
||||
&offdiag_z[cur_mag]);
|
||||
|
||||
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
|
||||
calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
|
||||
|
|
@ -599,7 +631,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
|
||||
fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
|
||||
fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
|
||||
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
|
||||
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets",
|
||||
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
|
||||
calibration_log_info(mavlink_log_pub, "Offsets: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)sphere_x[cur_mag],
|
||||
(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
|
||||
result = calibrate_return_ok;
|
||||
|
|
@ -654,7 +687,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
}
|
||||
|
||||
// Data points are no longer needed
|
||||
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||
free(worker_data.x[cur_mag]);
|
||||
free(worker_data.y[cur_mag]);
|
||||
free(worker_data.z[cur_mag]);
|
||||
|
|
@ -662,7 +695,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
|
||||
if (result == calibrate_return_ok) {
|
||||
|
||||
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||
if (device_ids[cur_mag] != 0) {
|
||||
struct mag_calibration_s mscale;
|
||||
mscale.x_scale = 1.0;
|
||||
|
|
@ -675,6 +708,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
// Set new scale
|
||||
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
|
||||
fd_mag = px4_open(str, 0);
|
||||
|
||||
if (fd_mag < 0) {
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag);
|
||||
result = calibrate_return_error;
|
||||
|
|
@ -686,6 +720,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
result = calibrate_return_error;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
if (result == calibrate_return_ok) {
|
||||
|
|
@ -694,18 +729,22 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
mscale.z_offset = sphere_z[cur_mag];
|
||||
|
||||
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
|
||||
|
||||
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != PX4_OK) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
|
||||
result = calibrate_return_error;
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
|
||||
|
||||
// Mag device no longer needed
|
||||
if (fd_mag >= 0) {
|
||||
px4_close(fd_mag);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
if (result == calibrate_return_ok) {
|
||||
|
|
@ -735,6 +774,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
if (failed) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
|
||||
result = calibrate_return_error;
|
||||
|
||||
} else {
|
||||
calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
|
||||
cur_mag,
|
||||
|
|
|
|||
Loading…
Reference in New Issue