fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 05:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

gyro_calibration: take into account temperature compensation when storing the scale

Browse Source
This commit is contained in:
Beat Küng
2017-02-01 08:40:23 +01:00
committed by Lorenz Meier
parent fbef2b7a6a
commit 7ebe2ac017
1 changed files with 44 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/gyro_calibration.cpp
+44 -12
View File
@@ -364,23 +364,55 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary))); failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));
for (unsigned s = 0; s < max_gyros; s++) { for (unsigned uorb_index = 0; uorb_index < max_gyros; uorb_index++) {
if (worker_data.device_id[s] != 0) { if (worker_data.device_id[uorb_index] != 0) {
char str[30]; char str[30];
(void)sprintf(str, "CAL_GYRO%u_XOFF", s); /* check if thermal compensation is enabled */
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].x_offset))); int32_t tc_enabled_int;
(void)sprintf(str, "CAL_GYRO%u_YOFF", s); param_get(param_find("TC_G_ENABLE"), &(tc_enabled_int));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset))); if (tc_enabled_int == 1) {
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s); /* Get struct containing sensor thermal compensation data */
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset))); struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
memset(&sensor_correction, 0, sizeof(sensor_correction));
orb_copy(ORB_ID(sensor_correction), worker_data.sensor_correction_sub, &sensor_correction);
(void)sprintf(str, "CAL_GYRO%u_ID", s); /* update the _X0_ terms to include the additional offset */
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s]))); int32_t handle;
float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f;
(void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index);
handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) {
val += worker_data.gyro_scale[uorb_index].x_offset;
} else if (axis_index == 1) {
val += worker_data.gyro_scale[uorb_index].y_offset;
} else if (axis_index == 2) {
val += worker_data.gyro_scale[uorb_index].z_offset;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
// Ensure the calibration values used the driver are at default settings
worker_data.gyro_scale[uorb_index].x_offset = 0.f;
worker_data.gyro_scale[uorb_index].y_offset = 0.f;
worker_data.gyro_scale[uorb_index].z_offset = 0.f;
}
(void)sprintf(str, "CAL_GYRO%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].x_offset)));
(void)sprintf(str, "CAL_GYRO%u_YOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].y_offset)));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].z_offset)));
(void)sprintf(str, "CAL_GYRO%u_ID", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.device_id[uorb_index])));
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
/* apply new scaling and offsets */ /* apply new scaling and offsets */
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, uorb_index);
int fd = px4_open(str, 0); int fd = px4_open(str, 0);
if (fd < 0) { if (fd < 0) {
@@ -388,7 +420,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
continue; continue;
} }
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]); res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[uorb_index]);
px4_close(fd); px4_close(fd);
if (res != PX4_OK) { if (res != PX4_OK) {