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

temperature_calibration: use device id from published topic

Browse Source 
uorb topic instance does not necessarily match the gyro device path instance,
so we need to use the id from the topic.
This commit is contained in:
Beat Küng
2017-01-25 22:43:29 +01:00
committed by Lorenz Meier
parent 00d2fc936a
commit be512fdc4c
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/events/temperature_calibration.cpp
+4 -4
View File
@@ -151,6 +151,7 @@ void Tempcal::task_main()
unsigned _hot_soak_sat[SENSOR_COUNT_MAX] = {};
unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
unsigned num_samples[SENSOR_COUNT_MAX] = {0};
uint32_t device_ids[SENSOR_COUNT_MAX] = {};
if (num_gyro > SENSOR_COUNT_MAX) {
num_gyro = SENSOR_COUNT_MAX;
@@ -196,6 +197,8 @@ void Tempcal::task_main()
if (fds[i].revents & POLLIN) {
orb_copy(ORB_ID(sensor_gyro), gyro_sub[i], &gyro_data);
device_ids[i] = gyro_data.device_id;
gyro_sample_filt[i][0] = gyro_data.x;//dat[l].GX;
gyro_sample_filt[i][1] = gyro_data.y;//dat[l].GY;
gyro_sample_filt[i][2] = gyro_data.z;//dat[l].GZ;
@@ -265,13 +268,10 @@ void Tempcal::task_main()
char str[30];
float param = 0.0f;
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, i);
int fd = px4_open(str, O_RDONLY);
int32_t device_id = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
int result = PX4_OK;
sprintf(str, "TC_G%d_ID", i);
result = param_set(param_find(str), &device_id);
result = param_set(param_find(str), &device_ids[i]);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);