diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake index bca8e7f3bf..aa1fc6b34a 100644 --- a/cmake/configs/qurt_eagle_release.cmake +++ b/cmake/configs/qurt_eagle_release.cmake @@ -41,7 +41,6 @@ set(config_module_list ${EAGLE_DRIVERS_SRC}/uart_esc ${EAGLE_DRIVERS_SRC}/rc_receiver ${EAGLE_DRIVERS_SRC}/csr_gps - ${EAGLE_DRIVERS_SRC}/utils # # System commands @@ -71,7 +70,7 @@ set(config_module_list modules/uORB modules/commander modules/controllib - + # # Libraries # diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 165c60d6ea..b3eef8f9aa 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -230,19 +230,19 @@ extern "C" { int px4_ioctl(int fd, int cmd, unsigned long arg) { - PX4_DEBUG("px4_ioctl fd = %d", fd); int ret = 0; VDev *dev = get_vdev(fd); if (dev) { ret = dev->ioctl(filemap[fd], cmd, arg); - } else { + PX4_ERR("px4_ioctl: vdev handle not available, file handle: %d", fd); ret = -EINVAL; } if (ret < 0) { + PX4_ERR("px4_ioctl: call failed: %d", ret); px4_errno = -ret; } @@ -305,6 +305,9 @@ extern "C" { fd_pollable = true; } } + else { + PX4_DEBUG("vdev invalid, index: %d, name: %s, handle: 0x%X", i, thread_name, fds[i].fd); + } } // If any FD can be polled, lock the semaphore and diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 796151c2f2..e7e12c9db2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,11 +41,14 @@ * well instead of relying on the sensor_combined topic. * * @author Lorenz Meier - * @author Julian Oes + * @author Julian Oes * @author Thomas Gubler * @author Anton Babushkin */ +// TODO-JYW: TESTING-TESTING +#define DEBUG_BUILD 1 + #include #include @@ -2152,15 +2155,26 @@ Sensors::task_main() _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], &raw.gyro_priority[0], &raw.gyro_errcount[0]); + warnx("gyro: sub: 0x%X, priority: 0x%X, error count: 0x%X", + _gyro_sub[0], raw.gyro_priority[0], raw.gyro_errcount[0]); _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); + warnx("mag: sub: 0x%X, priority: 0x%X, error count: 0x%X", + _mag_sub[0], raw.magnetometer_priority[0], raw.magnetometer_errcount[0]); _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); + warnx("gyro: sub: 0x%X, priority: 0x%X, error count: 0x%X", + _accel_sub[0], raw.accelerometer_priority[0], raw.accelerometer_errcount[0]); _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], &raw.baro_priority[0], &raw.baro_errcount[0]); + warnx("gyro: sub: 0x%X, priority: 0x%X, error count: 0x%X", + _baro_sub[0], raw.baro_priority[0], raw.baro_errcount[0]); + + warnx("subscription counts: gyro: %d, mag: %d, accel: %d, baro: %d", _gyro_count, _mag_count, + _accel_count, _baro_count); if (gcount_prev != _gyro_count || mcount_prev != _mag_count || @@ -2171,6 +2185,9 @@ Sensors::task_main() parameter_update_poll(true); } + warnx("counts: gyro: %d, mag: %d, accel: %d, baro: %d", _gyro_count, _mag_count, + _accel_count, _baro_count); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -2228,7 +2245,7 @@ Sensors::task_main() /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); + warnx("poll error %d, %d", pret, errno); continue; } @@ -2247,6 +2264,7 @@ Sensors::task_main() /* Work out if main gyro timed out and fail over to alternate gyro. * However, don't do this if the secondary is not available. */ if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000 && _gyro_sub[1] >= 0) { + warnx("gyro has timed out"); /* If the secondary failed as well, go to the tertiary, also only if available. */ if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000 && _gyro_sub[2] >= 0) {