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

calibration: bugs fixed, mavlink messages cleanup

This commit is contained in:
Anton Babushkin 2013-10-21 20:07:47 +02:00
parent ed79b686c5
commit ea89f23c91
4 changed files with 266 additions and 210 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/drv_accel.h
View File

@ -66,7 +66,7 @@ struct accel_report {
int16_t temperature_raw; int16_t temperature_raw;
}; };
/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */ /** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
struct accel_scale { struct accel_scale {
float x_offset; float x_offset;
float x_scale; float x_scale;

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

@ -100,6 +100,24 @@
* accel_T = A^-1 * g * accel_T = A^-1 * g
* g = 9.80665 * g = 9.80665
* *
* ===== Rotation =====
*
* Calibrating using model:
* accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r)
*
* Actual correction:
* accel_corr = rot * accel_T * (accel_raw - accel_offs)
*
* Known: accel_T_r, accel_offs_r, rot
* Unknown: accel_T, accel_offs
*
* Solution:
* accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs)
* rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs)
* rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs)
* => accel_T = rot^-1 * accel_T_r * rot
* => accel_offs = rot^-1 * accel_offs_r
*
* @author Anton Babushkin <anton.babushkin@me.com> * @author Anton Babushkin <anton.babushkin@me.com>
*/ */
@ -137,72 +155,97 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
int do_accel_calibration(int mavlink_fd) int do_accel_calibration(int mavlink_fd)
{ {
/* announce change */ mavlink_log_info(mavlink_fd, "accel calibration: started");
mavlink_log_info(mavlink_fd, "accel calibration started"); mavlink_log_info(mavlink_fd, "accel calibration: progress <0>");
mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
struct accel_scale accel_scale = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int res = OK;
/* reset all offsets to zero and all scales to one */
int fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets");
}
/* measure and calculate offsets & scales */ /* measure and calculate offsets & scales */
float accel_offs[3]; float accel_offs[3];
float accel_T[3][3]; float accel_T[3][3];
int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
if (res == OK) { if (res == OK) {
/* measurements complete successfully, rotate calibration values */ /* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT"); param_t board_rotation_h = param_find("SENS_BOARD_ROT");
enum Rotation board_rotation_id; int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_id)); param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix board_rotation(3, 3); math::Matrix board_rotation(3, 3);
get_rot_matrix(board_rotation_id, &board_rotation); get_rot_matrix(board_rotation_id, &board_rotation);
board_rotation = board_rotation.transpose(); math::Matrix board_rotation_t = board_rotation.transpose();
math::Vector3 accel_offs_vec; math::Vector3 accel_offs_vec;
accel_offs_vec.set(&accel_offs[0]); accel_offs_vec.set(&accel_offs[0]);
math::Vector3 accel_offs_rotated = board_rotation * accel_offs_vec; math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix accel_T_mat(3, 3); math::Matrix accel_T_mat(3, 3);
accel_T_mat.set(&accel_T[0][0]); accel_T_mat.set(&accel_T[0][0]);
math::Matrix accel_T_rotated = board_rotation.transpose() * accel_T_mat * board_rotation; math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2);
/* set parameters */ /* set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); mavlink_log_critical(mavlink_fd, "ERROR: setting accel params failed");
res = ERROR;
} }
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
accel_offs_rotated(0),
accel_T_rotated(0, 0),
accel_offs_rotated(1),
accel_T_rotated(1, 1),
accel_offs_rotated(2),
accel_T_rotated(2, 2),
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
mavlink_log_info(mavlink_fd, "accel calibration done");
return OK;
} else {
/* measurements error */
mavlink_log_info(mavlink_fd, "accel calibration aborted");
return ERROR;
} }
/* exit accel calibration mode */ if (res == OK) {
/* apply new scaling and offsets */
int fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for accel");
}
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters");
}
}
if (res == OK) {
mavlink_log_info(mavlink_fd, "accel calibration: done");
} else {
mavlink_log_info(mavlink_fd, "accel calibration: failed");
}
return res;
} }
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3])
@ -212,27 +255,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
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-" };
/* reset existing calibration */
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null);
close(fd);
if (OK != ioctl_res) {
warn("ERROR: failed to set scale / offsets for accel");
return ERROR;
}
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
unsigned done_count = 0; unsigned done_count = 0;
int res = OK;
while (true) { while (true) {
bool done = true; bool done = true;
@ -245,6 +271,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
} }
} }
if (old_done_count != done_count)
mavlink_log_info(mavlink_fd, "accel calibration: progress <%u>", 17 * done_count);
if (done)
break;
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "", (!data_collected[0]) ? "x+ " : "",
(!data_collected[1]) ? "x- " : "", (!data_collected[1]) ? "x- " : "",
@ -253,17 +285,11 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
(!data_collected[4]) ? "z+ " : "", (!data_collected[4]) ? "z+ " : "",
(!data_collected[5]) ? "z- " : ""); (!data_collected[5]) ? "z- " : "");
if (old_done_count != done_count)
mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count);
if (done)
break;
int orient = detect_orientation(mavlink_fd, sensor_combined_sub); int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0) { if (orient < 0) {
close(sensor_combined_sub); res = ERROR;
return ERROR; break;
} }
if (data_collected[orient]) { if (data_collected[orient]) {
@ -284,15 +310,16 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
close(sensor_combined_sub); close(sensor_combined_sub);
/* calculate offsets and transform matrix */ if (res == OK) {
int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); /* calculate offsets and transform matrix */
res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (res != 0) { if (res != OK) {
mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
return ERROR; }
} }
return OK; return res;
} }
/* /*
@ -309,7 +336,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
/* max-hold dispersion of accel */ /* max-hold dispersion of accel */
float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
/* EMA time constant in seconds*/ /* EMA time constant in seconds*/
float ema_len = 0.2f; float ema_len = 0.5f;
/* set "still" threshold to 0.25 m/s^2 */ /* set "still" threshold to 0.25 m/s^2 */
float still_thr2 = pow(0.25f, 2); float still_thr2 = pow(0.25f, 2);
/* set accel error threshold to 5m/s^2 */ /* set accel error threshold to 5m/s^2 */
@ -342,8 +369,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
float w = dt / ema_len; float w = dt / ema_len;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; float d = sensor.accelerometer_m_s2[i] - accel_ema[i];
float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; accel_ema[i] += d * w;
d = d * d; d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w); accel_disp[i] = accel_disp[i] * (1.0f - w);
@ -389,8 +416,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
} }
if (poll_errcount > 1000) { if (poll_errcount > 1000) {
mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor"); mavlink_log_critical(mavlink_fd, "ERROR: failed reading sensor");
return -1; return ERROR;
} }
} }
@ -424,9 +451,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
return 5; // [ 0, 0, -g ] return 5; // [ 0, 0, -g ]
mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
return -2; // Can't detect orientation return ERROR; // Can't detect orientation
} }
/* /*

54
src/modules/commander/commander.cpp
View File

@ -369,8 +369,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (hil_ret == OK && control_mode->flag_system_hil_enabled) { if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
/* reset the arming mode to disarmed */ /* reset the arming mode to disarmed */
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
if (arming_res != TRANSITION_DENIED) { if (arming_res != TRANSITION_DENIED) {
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
} else { } else {
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
} }
@ -481,27 +483,28 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break; break;
} }
case VEHICLE_CMD_COMPONENT_ARM_DISARM: case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
{ transition_result_t arming_res = TRANSITION_NOT_CHANGED;
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
if (safety->safety_switch_available && !safety->safety_off) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
} else { if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); if (safety->safety_switch_available && !safety->safety_off) {
} print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
if (arming_res == TRANSITION_CHANGED) { } else {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
result = VEHICLE_CMD_RESULT_ACCEPTED; }
} else {
mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); if (arming_res == TRANSITION_CHANGED) {
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} }
} }
}
break; break;
default: default:
@ -940,7 +943,7 @@ int commander_thread_main(int argc, char *argv[])
last_idle_time = system_load.tasks[0].total_runtime; last_idle_time = system_load.tasks[0].total_runtime;
/* check if board is connected via USB */ /* check if board is connected via USB */
struct stat statbuf; //struct stat statbuf;
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
} }
@ -970,6 +973,7 @@ int commander_thread_main(int argc, char *argv[])
if (armed.armed) { if (armed.armed) {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
} else { } else {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
} }
@ -1244,12 +1248,14 @@ int commander_thread_main(int argc, char *argv[])
counter++; counter++;
int blink_state = blink_msg_state(); int blink_state = blink_msg_state();
if (blink_state > 0) { if (blink_state > 0) {
/* blinking LED message, don't touch LEDs */ /* blinking LED message, don't touch LEDs */
if (blink_state == 2) { if (blink_state == 2) {
/* blinking LED message completed, restore normal state */ /* blinking LED message completed, restore normal state */
control_status_leds(&status, &armed, true); control_status_leds(&status, &armed, true);
} }
} else { } else {
/* normal state */ /* normal state */
control_status_leds(&status, &armed, status_changed); control_status_leds(&status, &armed, status_changed);
@ -1264,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[])
ret = pthread_join(commander_low_prio_thread, NULL); ret = pthread_join(commander_low_prio_thread, NULL);
if (ret) { if (ret) {
warn("join failed", ret); warn("join failed: %d", ret);
} }
rgbled_set_mode(RGBLED_MODE_OFF); rgbled_set_mode(RGBLED_MODE_OFF);
@ -1308,6 +1314,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
/* driving rgbled */ /* driving rgbled */
if (changed) { if (changed) {
bool set_normal_color = false; bool set_normal_color = false;
/* set mode */ /* set mode */
if (status->arming_state == ARMING_STATE_ARMED) { if (status->arming_state == ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON); rgbled_set_mode(RGBLED_MODE_ON);
@ -1332,6 +1339,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
rgbled_set_color(RGBLED_COLOR_AMBER); rgbled_set_color(RGBLED_COLOR_AMBER);
} }
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
} else { } else {
@ -1694,11 +1702,10 @@ void *commander_low_prio_loop(void *arg)
fds[0].events = POLLIN; fds[0].events = POLLIN;
while (!thread_should_exit) { while (!thread_should_exit) {
/* wait for up to 200ms for data */
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
/* timed out - periodic check for _task_should_exit, etc. */ /* timed out - periodic check for thread_should_exit, etc. */
if (pret == 0) if (pret == 0)
continue; continue;
@ -1773,7 +1780,7 @@ void *commander_low_prio_loop(void *arg)
} else if ((int)(cmd.param4) == 1) { } else if ((int)(cmd.param4) == 1) {
/* RC calibration */ /* RC calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_rc_calibration(mavlink_fd); calib_ret = do_rc_calibration(mavlink_fd);
} else if ((int)(cmd.param5) == 1) { } else if ((int)(cmd.param5) == 1) {
@ -1854,7 +1861,6 @@ void *commander_low_prio_loop(void *arg)
/* send acknowledge command */ /* send acknowledge command */
// XXX TODO // XXX TODO
} }
} }
close(cmd_sub); close(cmd_sub);

219
src/modules/commander/gyro_calibration.cpp
View File

@ -58,7 +58,7 @@ static const int ERROR = -1;
int do_gyro_calibration(int mavlink_fd) int do_gyro_calibration(int mavlink_fd)
{ {
mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); mavlink_log_info(mavlink_fd, "gyro calibration: started");
struct gyro_scale gyro_scale = { struct gyro_scale gyro_scale = {
0.0f, 0.0f,
@ -69,79 +69,85 @@ int do_gyro_calibration(int mavlink_fd)
1.0f, 1.0f,
}; };
int res = OK;
/* reset all offsets to zero and all scales to one */
int fd = open(GYRO_DEVICE_PATH, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets");
}
/* subscribe to gyro sensor topic */ /* subscribe to gyro sensor topic */
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
struct gyro_report gyro_report; struct gyro_report gyro_report;
/* reset all offsets to zero and all scales to one */ if (res == OK) {
int fd = open(GYRO_DEVICE_PATH, 0); /* determine gyro mean values */
const unsigned calibration_count = 5000;
unsigned calibration_counter = 0;
unsigned poll_errcount = 0;
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) while (calibration_counter < calibration_count) {
warn("WARNING: failed to reset scale / offsets for gyro"); /* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_gyro;
fds[0].events = POLLIN;
close(fd); int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
gyro_scale.x_offset += gyro_report.x;
gyro_scale.y_offset += gyro_report.y;
gyro_scale.z_offset += gyro_report.z;
calibration_counter++;
/*** --- OFFSETS --- ***/ if (calibration_counter % (calibration_count / 20) == 0)
mavlink_log_info(mavlink_fd, "gyro calibration: progress <%u>", (calibration_counter * 100) / calibration_count);
/* determine gyro mean values */ } else {
const unsigned calibration_count = 5000; poll_errcount++;
unsigned calibration_counter = 0; }
unsigned poll_errcount = 0;
while (calibration_counter < calibration_count) { if (poll_errcount > 1000) {
mavlink_log_critical(mavlink_fd, "ERROR: failed reading gyro sensor");
/* wait blocking for new data */ res = ERROR;
struct pollfd fds[1]; break;
fds[0].fd = sub_sensor_gyro; }
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
gyro_scale.x_offset += gyro_report.x;
gyro_scale.y_offset += gyro_report.y;
gyro_scale.z_offset += gyro_report.z;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count);
} else {
poll_errcount++;
} }
if (poll_errcount > 1000) { gyro_scale.x_offset /= calibration_count;
mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); gyro_scale.y_offset /= calibration_count;
close(sub_sensor_gyro); gyro_scale.z_offset /= calibration_count;
return ERROR; }
if (res == OK) {
/* check offsets */
if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
res = ERROR;
} }
} }
gyro_scale.x_offset /= calibration_count; if (res == OK) {
gyro_scale.y_offset /= calibration_count; /* set offset parameters to new values */
gyro_scale.z_offset /= calibration_count; if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
mavlink_log_info(mavlink_fd, "gyro offset calibration FAILED (NaN)"); mavlink_log_critical(mavlink_fd, "ERROR: setting gyro offs params failed");
close(sub_sensor_gyro); res = ERROR;
return ERROR; }
} }
/* beep on calibration end */ #if 0
mavlink_log_info(mavlink_fd, "gyro offset calibration done."); /* beep on offset calibration end */
mavlink_log_info(mavlink_fd, "gyro offset calibration done");
tune_neutral(); tune_neutral();
/* set offset parameters to new values */ /* scale calibration */
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
mavlink_log_critical(mavlink_fd, "Setting gyro offset parameters failed!");
}
/*** --- SCALING --- ***/
#if 0
/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */
mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
@ -163,9 +169,11 @@ int do_gyro_calibration(int mavlink_fd)
// XXX change to mag topic // XXX change to mag topic
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
uint64_t last_time = hrt_absolute_time(); uint64_t last_time = hrt_absolute_time();
@ -175,7 +183,7 @@ int do_gyro_calibration(int mavlink_fd)
/* abort this loop if not rotated more than 180 degrees within 5 seconds */ /* abort this loop if not rotated more than 180 degrees within 5 seconds */
if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
&& (hrt_absolute_time() - start_time > 5 * 1e6)) { && (hrt_absolute_time() - start_time > 5 * 1e6)) {
mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
close(sub_sensor_combined); close(sub_sensor_combined);
return OK; return OK;
@ -203,14 +211,17 @@ int do_gyro_calibration(int mavlink_fd)
// calculate error between estimate and measurement // calculate error between estimate and measurement
// apply declination correction for true heading as well. // apply declination correction for true heading as well.
//float mag = -atan2f(magNav(1),magNav(0)); //float mag = -atan2f(magNav(1),magNav(0));
float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag > M_PI_F) mag -= 2*M_PI_F;
if (mag < -M_PI_F) mag += 2*M_PI_F; if (mag > M_PI_F) mag -= 2 * M_PI_F;
if (mag < -M_PI_F) mag += 2 * M_PI_F;
float diff = mag - mag_last; float diff = mag - mag_last;
if (diff > M_PI_F) diff -= 2*M_PI_F; if (diff > M_PI_F) diff -= 2 * M_PI_F;
if (diff < -M_PI_F) diff += 2*M_PI_F;
if (diff < -M_PI_F) diff += 2 * M_PI_F;
baseline_integral += diff; baseline_integral += diff;
mag_last = mag; mag_last = mag;
@ -220,15 +231,15 @@ int do_gyro_calibration(int mavlink_fd)
// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); // warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
// } else if (poll_ret == 0) { // } else if (poll_ret == 0) {
// /* any poll failure for 1s is a reason to abort */ // /* any poll failure for 1s is a reason to abort */
// mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
// return; // return;
} }
} }
float gyro_scale = baseline_integral / gyro_integral; float gyro_scale = baseline_integral / gyro_integral;
warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
@ -236,42 +247,54 @@ int do_gyro_calibration(int mavlink_fd)
if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) {
mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)");
close(sub_sensor_gyro); close(sub_sensor_gyro);
mavlink_log_critical(mavlink_fd, "gyro calibration failed");
return ERROR; return ERROR;
} }
/* beep on calibration end */ /* beep on calibration end */
mavlink_log_info(mavlink_fd, "gyro scale calibration done."); mavlink_log_info(mavlink_fd, "gyro scale calibration done");
tune_neutral(); tune_neutral();
#endif #endif
/* set scale parameters to new values */
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
mavlink_log_critical(mavlink_fd, "Setting gyro scale parameters failed!");
}
/* apply new scaling and offsets */
fd = open(GYRO_DEVICE_PATH, 0);
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
warn("WARNING: failed to apply new scale for gyro");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warnx("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, "gyro store failed");
close(sub_sensor_gyro);
return ERROR;
}
mavlink_log_info(mavlink_fd, "gyro calibration done.");
close(sub_sensor_gyro); close(sub_sensor_gyro);
return OK;
if (res == OK) {
/* set scale parameters to new values */
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
mavlink_log_critical(mavlink_fd, "ERROR: setting gyro scale params failed");
res = ERROR;
}
}
if (res == OK) {
/* apply new scaling and offsets */
fd = open(GYRO_DEVICE_PATH, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for gyro");
}
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters");
}
}
if (res == OK) {
mavlink_log_info(mavlink_fd, "gyro calibration: done");
} else {
mavlink_log_info(mavlink_fd, "gyro calibration: failed");
}
return res;
} }