fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: fix bug in gyro calibration

Browse Source 
If the same gyro data was contained in two uORB instances, the thermal offset coefficient was being corrected twice.

TODO should fix what was causing data from the same sensor to appear on two uORB topics.
This commit is contained in:
Paul Riseborough 2017-01-28 12:18:56 +11:00 committed by Lorenz Meier
parent 6e841f6cbd
commit bdbc4f4d65
1 changed files with 32 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -364,6 +364,8 @@ 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)));
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 < max_gyros; uorb_index++) {
if (worker_data.device_id[uorb_index] != 0) {
char str[30];
@ -377,27 +379,38 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
memset(&sensor_correction, 0, sizeof(sensor_correction));
orb_copy(ORB_ID(sensor_correction), worker_data.sensor_correction_sub, &sensor_correction);
/* update the _X0_ terms to include the additional offset */
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;
}
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));
/* don't allow a parameter instance to be calibrated again by another uORB instance */
if (!tc_locked[sensor_correction.gyro_mapping[uorb_index]]) {
tc_locked[sensor_correction.gyro_mapping[uorb_index]] = true;
/* update the _X0_ terms to include the additional offset */
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;
}
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 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;