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

commander : warn if excess gyros are connected

Browse Source
This commit is contained in:
Mohammed Kabir
2017-05-27 00:31:05 +02:00
committed by Lorenz Meier
parent a11ef6be10
commit d177ccc9a7
1 changed files with 15 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/gyro_calibration.cpp
+15 -1
View File
@@ -294,9 +294,23 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
// 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++) {
// Warn that we will not calibrate more than max_gyros gyroscopes
if (gyro_count > max_gyros) {
calibration_log_critical(mavlink_log_pub, "[cal] Detected %u gyros, but will calibrate only %u", gyro_count, max_gyros);
}
for (unsigned s = 0; s < gyro_count && s < max_gyros; s++) {
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
if (worker_data.gyro_sensor_sub[s] < 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Gyro #%u not found, abort", s);
res = calibrate_return_error;
break;
}
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP)
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct gyro_report gyro_report;