zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

sbg
Paul Riseborough 8 years ago committed by Lorenz Meier
parent
06f280e021
commit
6e841f6cbd
1 changed files with 58 additions and 46 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 48
      src/modules/commander/accelerometer_calibration.cpp

48
src/modules/commander/accelerometer_calibration.cpp
Unescape 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,19 +336,24 @@ 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;
} }
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)); failed |= (PX4_OK != param_set_no_notification(handle, &val));
} }
}
/* update the _SCL_ terms to include the scale factor */
for (unsigned axis_index = 0; axis_index < 3; axis_index++) { for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 1.0f; val = 1.0f;
(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index); (void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str); handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) { if (axis_index == 0) {
val *= accel_scale.x_scale; val = accel_scale.x_scale;
} else if (axis_index == 1) { } else if (axis_index == 1) {
val *= accel_scale.y_scale; val = accel_scale.y_scale;
} else if (axis_index == 2) { } else if (axis_index == 2) {
val *= accel_scale.z_scale; 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));
@ -351,8 +361,9 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
failed |= (PX4_OK != param_set_no_notification(handle, &val)); failed |= (PX4_OK != param_set_no_notification(handle, &val));
} }
} }
}
// Ensure the calibration values used the driver are at default settings // 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;

Write Preview
Loading…
Cancel
Save