zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

abs/fabs bugfix, logging cleanup

sbg
Anton Babushkin 12 years ago
parent
593e3252dd
commit
26f9a1d42c
1 changed files with 29 additions and 51 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 80
      apps/commander/accelerometer_calibration.c

80
apps/commander/accelerometer_calibration.c
Unescape View File

@ -80,7 +80,7 @@
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) {
/* announce change */ /* announce change */
mavlink_log_info(mavlink_fd, "accelerometer calibration started"); mavlink_log_info(mavlink_fd, "accel calibration started");
/* set to accel calibration mode */ /* set to accel calibration mode */
status->flag_preflight_accel_calibration = true; status->flag_preflight_accel_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd); state_machine_publish(status_pub, status, mavlink_fd);
@ -96,7 +96,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!"); mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
} }
int fd = open(ACCEL_DEVICE_PATH, 0); int fd = open(ACCEL_DEVICE_PATH, 0);
@ -122,7 +122,6 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
} }
mavlink_log_info(mavlink_fd, "accel calibration done"); mavlink_log_info(mavlink_fd, "accel calibration done");
tune_confirm(); tune_confirm();
sleep(2); sleep(2);
tune_confirm(); tune_confirm();
@ -142,12 +141,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], float accel_scale[3]) { int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], float accel_scale[3]) {
const int samples_num = 2500; const int samples_num = 2500;
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int16_t accel_raw_ref[6][3]; int16_t accel_raw_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false }; bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
while (true) { while (true) {
bool done = true; bool done = true;
char str[80]; char str[80];
@ -159,25 +157,21 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3],
done = false; done = false;
} }
} }
if (done) { if (done)
mavlink_log_info(mavlink_fd, "all accel measurements complete");
break; break;
} else { mavlink_log_info(mavlink_fd, str);
mavlink_log_info(mavlink_fd, str);
int orient = detect_orientation(mavlink_fd, sensor_combined_sub); int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0) { if (orient < 0)
sprintf(str, "orientation detection error: %i", orient); return ERROR;
mavlink_log_info(mavlink_fd, str);
return ERROR; sprintf(str, "meas started: %s", orientation_strs[orient]);
} mavlink_log_info(mavlink_fd, str);
mavlink_log_info(mavlink_fd, "accel measurement started"); read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[orient][0]), samples_num);
read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[orient][0]), samples_num); str_ptr = sprintf(str, "meas result for %s: [ %i %i %i ]", orientation_strs[orient], accel_raw_ref[orient][0], accel_raw_ref[orient][1], accel_raw_ref[orient][2]);
//mavlink_log_info(mavlink_fd, "accel measurement complete"); mavlink_log_info(mavlink_fd, str);
str_ptr = sprintf(str, "complete: %i [ %i %i %i ]", orient, accel_raw_ref[orient][0], accel_raw_ref[orient][1], accel_raw_ref[orient][2]); data_collected[orient] = true;
mavlink_log_info(mavlink_fd, str); tune_confirm();
data_collected[orient] = true;
tune_confirm();
}
} }
close(sensor_combined_sub); close(sensor_combined_sub);
@ -186,26 +180,16 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3],
float accel_T[3][3]; float accel_T[3][3];
int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, CONSTANTS_ONE_G); int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (res != 0) { if (res != 0) {
mavlink_log_info(mavlink_fd, "calibration values calculation error"); mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error");
return ERROR; return ERROR;
} }
char str[80];
sprintf(str, "accel offsets: [ %i %i %i ]", accel_offs[0], accel_offs[1], accel_offs[2]);
mavlink_log_info(mavlink_fd, str);
//mavlink_log_info(mavlink_fd, "accel transform matrix:");
for (int i = 0; i < 3; i++) {
//sprintf(str, "\t[ %0.6f %0.6f %0.6f ]", accel_T[i][0], accel_T[i][1], accel_T[i][2]);
//mavlink_log_info(mavlink_fd, str);
}
/* convert raw accel offset to scaled and transform matrix to scales /* convert raw accel offset to scaled and transform matrix to scales
* rotation part of transform matrix is not used by now */ * rotation part of transform matrix is not used by now */
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
accel_offs_scaled[i] = accel_offs[i] * accel_T[i][i]; accel_offs_scaled[i] = accel_offs[i] * accel_T[i][i];
accel_scale[i] = accel_T[i][i]; accel_scale[i] = accel_T[i][i];
} }
return OK; return OK;
} }
@ -226,8 +210,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
float ema_len = 0.2f; float ema_len = 0.2f;
/* set "still" threshold to 0.005 m/s^2 */ /* set "still" threshold to 0.005 m/s^2 */
float still_thr2 = pow(0.05f / CONSTANTS_ONE_G, 2); float still_thr2 = pow(0.05f / CONSTANTS_ONE_G, 2);
/* set accel error threshold to 20% of accel vector length */ /* set accel error threshold to 30% of accel vector length */
float accel_err_thr = 0.2f; float accel_err_thr = 0.3f;
/* still time required in us */ /* still time required in us */
int64_t still_time = 2000000; int64_t still_time = 2000000;
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
@ -264,7 +248,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
/* is still now */ /* is still now */
if (t_still == 0) { if (t_still == 0) {
/* first time */ /* first time */
mavlink_log_info(mavlink_fd, "still"); mavlink_log_info(mavlink_fd, "still...");
t_still = t; t_still = t;
t_timeout = t + timeout; t_timeout = t + timeout;
} else { } else {
@ -279,7 +263,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
accel_disp[2] > still_thr_raw2 * 2.0f) { accel_disp[2] > still_thr_raw2 * 2.0f) {
/* not still, reset still start time */ /* not still, reset still start time */
if (t_still != 0) { if (t_still != 0) {
mavlink_log_info(mavlink_fd, "moving"); mavlink_log_info(mavlink_fd, "moving...");
t_still = 0; t_still = 0;
} }
} }
@ -289,17 +273,11 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
return -3; return -3;
} }
if (t > t_timeout) { if (t > t_timeout) {
mavlink_log_info(mavlink_fd, "ERROR: timeout");
return -1; return -1;
} }
} }
float accel_len = sqrt(accel_len2); float accel_len = sqrt(accel_len2);
float accel_err_thr_raw = accel_len * accel_err_thr; float accel_err_thr_raw = accel_len * accel_err_thr;
char str[80];
sprintf(str, "ema: [ %.1f %.1f %.1f ]", accel_ema[0], accel_ema[1], accel_ema[2]);
mavlink_log_info(mavlink_fd, str);
sprintf(str, "disp: [ %.1f %.1f %.1f ]", accel_disp[0], accel_disp[1], accel_disp[2]);
mavlink_log_info(mavlink_fd, str);
if ( fabs(accel_ema[0] - accel_len) < accel_err_thr_raw && if ( fabs(accel_ema[0] - accel_len) < accel_err_thr_raw &&
fabs(accel_ema[1]) < accel_err_thr_raw && fabs(accel_ema[1]) < accel_err_thr_raw &&
fabs(accel_ema[2]) < accel_err_thr_raw ) fabs(accel_ema[2]) < accel_err_thr_raw )
@ -316,13 +294,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
fabs(accel_ema[1] + accel_len) < accel_err_thr_raw && fabs(accel_ema[1] + accel_len) < accel_err_thr_raw &&
fabs(accel_ema[2]) < accel_err_thr_raw ) fabs(accel_ema[2]) < accel_err_thr_raw )
return 3; // [ 0, -g, 0 ] return 3; // [ 0, -g, 0 ]
if ( abs(accel_ema[0]) < accel_err_thr_raw && if ( fabs(accel_ema[0]) < accel_err_thr_raw &&
abs(accel_ema[1]) < accel_err_thr_raw && fabs(accel_ema[1]) < accel_err_thr_raw &&
abs(accel_ema[2] - accel_len) < accel_err_thr_raw ) fabs(accel_ema[2] - accel_len) < accel_err_thr_raw )
return 4; // [ 0, 0, g ] return 4; // [ 0, 0, g ]
if ( abs(accel_ema[0]) < accel_err_thr_raw && if ( fabs(accel_ema[0]) < accel_err_thr_raw &&
abs(accel_ema[1]) < accel_err_thr_raw && fabs(accel_ema[1]) < accel_err_thr_raw &&
abs(accel_ema[2] + accel_len) < accel_err_thr_raw ) fabs(accel_ema[2] + accel_len) < accel_err_thr_raw )
return 5; // [ 0, 0, -g ] return 5; // [ 0, 0, -g ]
mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
return -2; // Can't detect orientation return -2; // Can't detect orientation

Write Preview
Loading…
Cancel
Save