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

Commander: Fix accel cal duration

This commit is contained in:
Lorenz Meier 2015-12-21 13:44:51 +01:00
parent 2041e3f3bc
commit e525ab3b73
1 changed files with 20 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -326,7 +326,7 @@ int do_accel_calibration(int mavlink_fd)
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
const unsigned samples_num = 3000;
const unsigned samples_num = 750;
accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
@ -574,7 +574,8 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
int do_level_calibration(int mavlink_fd) {
const unsigned cal_time = 5;
const unsigned cal_hz = 100;
const unsigned settle_time = 30;
unsigned settle_time = 30;
bool success = false;
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s att;
@ -582,18 +583,25 @@ int do_level_calibration(int mavlink_fd) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "level");
param_t roll_offset_handler = param_find("SENS_BOARD_X_OFF");
param_t pitch_offset_handler = param_find("SENS_BOARD_Y_OFF");
param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
param_t board_rot_handle = param_find("SENS_BOARD_ROT");
// 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);
int32_t board_rot_current = 0;
param_get(roll_offset_handle, &roll_offset_current);
param_get(pitch_offset_handle, &pitch_offset_current);
param_get(board_rot_handle, &board_rot_current);
if (board_rot_current == 0) {
settle_time = 0;
}
float zero = 0.0f;
param_set(roll_offset_handler, &zero);
param_set(pitch_offset_handler, &zero);
param_set(roll_offset_handle, &zero);
param_set(pitch_offset_handle, &zero);
px4_pollfd_struct_t fds[1];
@ -647,8 +655,8 @@ int do_level_calibration(int mavlink_fd) {
} 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);
param_set(roll_offset_handle, &roll_mean);
param_set(pitch_offset_handle, &pitch_mean);
success = true;
}
@ -658,8 +666,8 @@ out:
return 0;
} else {
// set old parameters
param_set(roll_offset_handler, &roll_offset_current);
param_set(pitch_offset_handler, &pitch_offset_current);
param_set(roll_offset_handle, &roll_offset_current);
param_set(pitch_offset_handle, &pitch_offset_current);
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level");
return 1;
}