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

accelerometer_calibration: make sure to initialize sensor_correction properly

Browse Source
This commit is contained in:
Beat Küng
2017-02-01 08:40:23 +01:00
committed by Lorenz Meier
parent 51def4fc60
commit 69fd8447ae
1 changed files with 10 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+10 -11
View File
@@ -465,7 +465,6 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false }; bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
// Initialise sub to sensor thermal compensation data // Initialise sub to sensor thermal compensation data
worker_data.sensor_correction_sub = -1;
worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
// Initialize subs to error condition so we know which ones are open and which are not // Initialize subs to error condition so we know which ones are open and which are not
@@ -588,18 +587,18 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m
memset(accel_sum, 0, sizeof(accel_sum)); memset(accel_sum, 0, sizeof(accel_sum));
unsigned errcount = 0; unsigned errcount = 0;
/* Define struct containing sensor thermal compensation data and set default values */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */ struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
memset(&sensor_correction, 0, sizeof(sensor_correction));
for (unsigned i=0; i<3; i++) {
sensor_correction.accel_scale_0[i] = 1.0f;
sensor_correction.accel_scale_1[i] = 1.0f;
sensor_correction.accel_scale_2[i] = 1.0f;
}
/* get latest thermal corrections */ /* try to get latest thermal corrections */
orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction); if (orb_copy(ORB_ID(sensor_correction), 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.accel_scale_0[i] = 1.0f;
sensor_correction.accel_scale_1[i] = 1.0f;
sensor_correction.accel_scale_2[i] = 1.0f;
}
}
/* use the first sensor to pace the readout, but do per-sensor counts */ /* use the first sensor to pace the readout, but do per-sensor counts */
while (counts[0] < samples_num) { while (counts[0] < samples_num) {