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

commander: fix the calibration for NuttX.

Browse Source 
This fixes some regressions on the calibration using NuttX, especially
considering the PRIME param and the device ids.
This commit is contained in:
Julian Oes
2016-03-24 13:08:31 +01:00
parent d0e9ec0c67
commit 88a812618c
3 changed files with 70 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+21 -7
View File
@@ -410,25 +410,39 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
uint64_t timestamps[max_accel_sens];
for (unsigned i = 0; i < max_accel_sens; i++) {
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned accel_count = orb_group_count(ORB_ID(sensor_accel));
for (unsigned i = 0; i < accel_count; i++) {
worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
if (worker_data.subs[i] < 0) {
result = calibrate_return_error;
break;
}
#ifdef __PX4_QURT
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);
device_id[i] = accel_report.device_id;
#endif
/* store initial timestamp - used to infer which sensors are active */
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
timestamps[i] = arp.timestamp;
// Get priority
int32_t prio;
orb_priority(worker_data.subs[i], &prio);
if (device_id[i] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.subs[i], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[i];
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[i];
}
} else {
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Accel #%u no device id, abort", i);
result = calibrate_return_error;
break;
}
}
src/modules/commander/gyro_calibration.cpp
+23 -8
View File
@@ -173,8 +173,10 @@ int do_gyro_calibration(int mavlink_fd)
for (unsigned s = 0; s < max_gyros; s++) {
char str[30];
// Reset gyro ids to unavailable
// Reset gyro ids to unavailable.
worker_data.device_id[s] = 0;
// And set default subscriber values.
worker_data.gyro_sensor_sub[s] = -1;
(void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
if (res != OK) {
@@ -232,16 +234,29 @@ int do_gyro_calibration(int mavlink_fd)
}
for (unsigned s = 0; s < max_gyros; s++) {
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned gyro_count = orb_group_count(ORB_ID(sensor_gyro));
for (unsigned s = 0; s < gyro_count; s++) {
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
#ifdef __PX4_QURT
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report);
worker_data.device_id[s] = gyro_report.device_id;
#endif
// Get priority
int32_t prio;
orb_priority(worker_data.gyro_sensor_sub[s], &prio);
if (worker_data.device_id[s] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.gyro_sensor_sub[s], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = worker_data.device_id[s];
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = worker_data.device_id[s];
}
} else {
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Gyro #%u no device id, abort", s);
}
}
src/modules/commander/mag_calibration.cpp
+26 -9
View File
@@ -468,16 +468,27 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
// Setup subscriptions to mag sensors
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
if (worker_data.sub_mag[cur_mag] < 0) {
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag);
result = calibrate_return_error;
break;
}
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag));
for (unsigned cur_mag = 0; cur_mag < mag_count; cur_mag++) {
// Mag in this slot is available
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
#ifdef __PX4_QURT
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
device_ids[cur_mag] = mag_report.device_id;
#endif
if (worker_data.sub_mag[cur_mag] < 0) {
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag);
result = calibrate_return_error;
break;
}
if (device_ids[cur_mag] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.sub_mag[cur_mag], &prio);
@@ -486,6 +497,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
device_prio_max = prio;
device_id_primary = device_ids[cur_mag];
}
} else {
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag);
result = calibrate_return_error;
break;
}
}
}
@@ -696,9 +711,11 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
#ifndef __PX4_QURT
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
#endif
}
}
}