Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

commander: fix bugs in handling of thermal compensation during access cal

Browse Source
This commit is contained in:
Paul Riseborough
2017-01-28 11:43:37 +11:00
committed by Lorenz Meier
parent 06f280e021
commit 6e841f6cbd
1 changed files with 58 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+40 -28
View File
@@ -275,6 +275,8 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
get_rot_matrix(board_rotation_id, &board_rotation); get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator
for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) { for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {
/* handle individual sensors, one by one */ /* handle individual sensors, one by one */
@@ -295,11 +297,11 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_offset, (double)accel_scale.x_offset,
(double)accel_scale.y_offset, (double)accel_scale.y_offset,
(double)accel_scale.z_offset); (double)accel_scale.z_offset);
PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_scale, (double)accel_scale.x_scale,
(double)accel_scale.y_scale, (double)accel_scale.y_scale,
(double)accel_scale.z_scale); (double)accel_scale.z_scale);
@@ -315,8 +317,11 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction); orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction);
orb_unsubscribe(sensor_correction_sub); orb_unsubscribe(sensor_correction_sub);
/* don't allow a parameter instance to be calibrated more than once by another uORB instance */
if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {
tc_locked[sensor_correction.accel_mapping[uorb_index]] = true;
/* update the _X0_ terms to include the additional offset */ /* update the _X0_ terms to include the additional offset */
/* update the _SCL_ terms to include the additonal scale factor */
int32_t handle; int32_t handle;
float val; float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) { for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
@@ -331,20 +336,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
} else if (axis_index == 2) { } else if (axis_index == 2) {
val += accel_scale.z_offset; val += accel_scale.z_offset;
} }
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 1.0f;
(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) {
val *= accel_scale.x_scale;
} else if (axis_index == 1) {
val *= accel_scale.y_scale;
} else if (axis_index == 2) {
val *= accel_scale.z_scale;
}
if (axis_index == 2) { //notify the system about the change, but only once, for the last one if (axis_index == 2) { //notify the system about the change, but only once, for the last one
failed |= (PX4_OK != param_set(handle, &val)); failed |= (PX4_OK != param_set(handle, &val));
} else { } else {
@@ -352,7 +343,27 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
} }
} }
// Ensure the calibration values used the driver are at default settings /* update the _SCL_ terms to include the scale factor */
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 1.0f;
(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
if (axis_index == 0) {
val = accel_scale.x_scale;
} else if (axis_index == 1) {
val = accel_scale.y_scale;
} else if (axis_index == 2) {
val = accel_scale.z_scale;
}
if (axis_index == 2) { //notify the system about the change, but only once, for the last one
failed |= (PX4_OK != param_set(handle, &val));
} else {
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
}
}
// Ensure the calibration values used by the driver are at default settings when we are using thermal calibration data
accel_scale.x_offset = 0.f; accel_scale.x_offset = 0.f;
accel_scale.y_offset = 0.f; accel_scale.y_offset = 0.f;
accel_scale.z_offset = 0.f; accel_scale.z_offset = 0.f;
@@ -361,6 +372,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
accel_scale.z_scale = 1.f; accel_scale.z_scale = 1.f;
} }
// save the driver level calibration data
(void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index); (void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index); (void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
@@ -606,19 +618,19 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m
struct accel_report arp; struct accel_report arp;
orb_copy(ORB_ID(sensor_accel), subs[s], &arp); orb_copy(ORB_ID(sensor_accel), subs[s], &arp);
// Apply thermal corrections // Apply thermal offset corrections in sensor/board frame
if (s == 0) { if (s == 0) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]) * sensor_correction.accel_scale_0[0]; accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]) * sensor_correction.accel_scale_0[1]; accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]) * sensor_correction.accel_scale_0[2]; accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]);
} else if (s == 1) { } else if (s == 1) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]) * sensor_correction.accel_scale_1[0]; accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]) * sensor_correction.accel_scale_1[1]; accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]) * sensor_correction.accel_scale_1[2]; accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]);
} else if (s == 2) { } else if (s == 2) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]) * sensor_correction.accel_scale_2[0]; accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]) * sensor_correction.accel_scale_2[1]; accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]) * sensor_correction.accel_scale_2[2]; accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]);
} else { } else {
accel_sum[s][0] += arp.x; accel_sum[s][0] += arp.x;
accel_sum[s][1] += arp.y; accel_sum[s][1] += arp.y;