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

gyro_calibration: make sure to initialize sensor_correction properly

Browse Source 
if it's not published yet or published with low frequency, this makes sure
we have valid data.

also:
- _sensor_correction -> sensor_correction
- remove unnecessary init of sensor_correction_sub
This commit is contained in:
Beat Küng 2017-01-25 13:43:38 +01:00 committed by Lorenz Meier
parent f890c82c97
commit 51def4fc60
1 changed files with 23 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -82,9 +82,16 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
struct gyro_report gyro_report;
unsigned poll_errcount = 0;
// set up subscription to sensor thermal compensation data
struct sensor_correction_s _sensor_correction; /**< sensor thermal corrections */
memset(&_sensor_correction, 0, sizeof(_sensor_correction));
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) {
/* use default values */
memset(&sensor_correction, 0, sizeof(sensor_correction));
for (unsigned i = 0; i < 3; i++) {
sensor_correction.gyro_scale_0[i] = 1.0f;
sensor_correction.gyro_scale_1[i] = 1.0f;
sensor_correction.gyro_scale_2[i] = 1.0f;
}
}
px4_pollfd_struct_t fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) {
@ -106,7 +113,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
orb_check(worker_data->sensor_correction_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &_sensor_correction);
orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction);
}
int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
@ -122,25 +129,25 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
if (s == 0) {
// take a working copy
worker_data->gyro_scale[s].x_offset += (gyro_report.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2];
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2];
// take a reference copy of the primary sensor including correction for thermal drift
orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0);
worker_data->gyro_report_0.x = (gyro_report.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0];
worker_data->gyro_report_0.y = (gyro_report.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1];
worker_data->gyro_report_0.z = (gyro_report.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2];
worker_data->gyro_report_0.x = (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0];
worker_data->gyro_report_0.y = (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1];
worker_data->gyro_report_0.z = (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2];
} else if (s == 1) {
worker_data->gyro_scale[s].x_offset += (gyro_report.x - _sensor_correction.gyro_offset_1[0]) * _sensor_correction.gyro_scale_1[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - _sensor_correction.gyro_offset_1[2]) * _sensor_correction.gyro_scale_1[2];
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_1[0]) * sensor_correction.gyro_scale_1[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_1[1]) * sensor_correction.gyro_scale_1[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_1[2]) * sensor_correction.gyro_scale_1[2];
} else if (s == 2) {
worker_data->gyro_scale[s].x_offset += (gyro_report.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2];
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_2[0]) * sensor_correction.gyro_scale_2[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_2[1]) * sensor_correction.gyro_scale_2[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_2[2]) * sensor_correction.gyro_scale_2[2];
} else {
worker_data->gyro_scale[s].x_offset += gyro_report.x;
@ -202,7 +209,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
int device_prio_max = 0;
int32_t device_id_primary = 0;
worker_data.sensor_correction_sub = -1;
worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
for (unsigned s = 0; s < max_gyros; s++) {