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

Cherry-picked i22438c8

Browse Source 
Original description:
Modified the sensor module to prevent the selection of an invalid
secondary/tertiary gyro if the primary gyro times out
This commit is contained in:
jwilson 2016-02-05 19:33:37 -08:00 committed by Julian Oes
parent e26cca760a
commit b31472af0c
3 changed files with 27 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cmake/configs/qurt_eagle_release.cmake
View File

@ -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
#

7
src/drivers/device/vdev_posix.cpp
View File

@ -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

24
src/modules/sensors/sensors.cpp
View File

@ -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 <lorenz@px4.io>
* @author Julian Oes <julian@oes.ch>
* @author Julian Oes <julian@px4.io>
* @author Thomas Gubler <thomas@px4.io>
* @author Anton Babushkin <anton@px4.io>
*/
// TODO-JYW: TESTING-TESTING
#define DEBUG_BUILD 1
#include <board_config.h>
#include <px4_config.h>
@ -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) {