dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

reset board rotation offset params if level calibration failed

This commit is contained in:
Roman Bapst 2015-05-19 16:22:52 +02:00 committed by Lorenz Meier
parent 11564a0f14
commit e7a522edbc
1 changed files with 19 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/commander/accelerometer_calibration.cpp
View File

@ -568,6 +568,12 @@ int do_level_calibration(int mavlink_fd) {
param_t roll_offset_handler = param_find("SENS_BOARD_X_OFF");
param_t pitch_offset_handler = param_find("SENS_BOARD_Y_OFF");
// save old values if calibration fails
float roll_offset_current;
float pitch_offset_current;
param_get(roll_offset_handler, &roll_offset_current);
param_get(pitch_offset_handler, &pitch_offset_current);
float zero = 0.0f;
param_set(roll_offset_handler, &zero);
param_set(pitch_offset_handler, &zero);
@ -600,28 +606,34 @@ int do_level_calibration(int mavlink_fd) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
bool success = false;
if (counter > (cal_time * cal_hz / 2 )) {
roll_mean /= counter;
pitch_mean /= counter;
} else {
mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken");
return 1;
}
if (fabsf(roll_mean) > 0.8f ) {
mavlink_and_console_log_critical(mavlink_fd, "excess roll angle");
return 1;
} else if (fabsf(pitch_mean) > 0.8f ) {
mavlink_and_console_log_critical(mavlink_fd, "excess pitch angle");
return 1;
}
else {
} else {
roll_mean *= (float)M_RAD_TO_DEG;
pitch_mean *= (float)M_RAD_TO_DEG;
param_set(roll_offset_handler, &roll_mean);
param_set(pitch_offset_handler, &pitch_mean);
success = true;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level");
return 0;
if (success) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level");
return 0;
} else {
// set old parameters
param_set(roll_offset_handler, &roll_offset_current);
param_set(pitch_offset_handler, &pitch_offset_current);
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level");
return 1;
}
}