dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Added rate limit in sensors app. Just pending accel filters now

This commit is contained in:
Lorenz Meier 2013-08-04 19:10:56 +02:00
parent f45e74265e
commit 36cca7a31b
1 changed files with 63 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -92,8 +92,35 @@
#define BARO_HEALTH_COUNTER_LIMIT_OK 5 #define BARO_HEALTH_COUNTER_LIMIT_OK 5
#define ADC_HEALTH_COUNTER_LIMIT_OK 5 #define ADC_HEALTH_COUNTER_LIMIT_OK 5
#define ADC_BATTERY_VOLTAGE_CHANNEL 10 /**
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 * Analog layout:
* FMU:
* IN2 - battery voltage
* IN3 - battery current
* IN4 - 5V sense
* IN10 - spare (we could actually trim these from the set)
* IN11 - spare (we could actually trim these from the set)
* IN12 - spare (we could actually trim these from the set)
* IN13 - aux1
* IN14 - aux2
* IN15 - pressure sensor
*
* IO:
* IN4 - servo supply rail
* IN5 - analog RSSI
*/
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#define ADC_BATTERY_VOLTAGE_CHANNEL 2
#define ADC_BATTERY_CURRENT_CHANNEL 3
#define ADC_5V_RAIL_SENSE 4
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
#endif
#define BAT_VOL_INITIAL 0.f #define BAT_VOL_INITIAL 0.f
#define BAT_VOL_LOWPASS_1 0.99f #define BAT_VOL_LOWPASS_1 0.99f
@ -731,12 +758,27 @@ Sensors::accel_init()
errx(1, "FATAL: no accelerometer found"); errx(1, "FATAL: no accelerometer found");
} else { } else {
// XXX do the check more elegantly
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* set the accel internal sampling rate up to at leat 500Hz */ /* set the accel internal sampling rate up to at leat 500Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 500); ioctl(fd, ACCELIOCSSAMPLERATE, 500);
/* set the driver to poll at 500Hz */ /* set the driver to poll at 500Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 500); ioctl(fd, SENSORIOCSPOLLRATE, 500);
#else
/* set the accel internal sampling rate up to at leat 800Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 800);
/* set the driver to poll at 800Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 800);
#endif
warnx("using system accel"); warnx("using system accel");
close(fd); close(fd);
} }
@ -754,12 +796,27 @@ Sensors::gyro_init()
errx(1, "FATAL: no gyro found"); errx(1, "FATAL: no gyro found");
} else { } else {
// XXX do the check more elegantly
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* set the gyro internal sampling rate up to at leat 500Hz */ /* set the gyro internal sampling rate up to at leat 500Hz */
ioctl(fd, GYROIOCSSAMPLERATE, 500); ioctl(fd, GYROIOCSSAMPLERATE, 500);
/* set the driver to poll at 500Hz */ /* set the driver to poll at 500Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 500); ioctl(fd, SENSORIOCSPOLLRATE, 500);
#else
/* set the gyro internal sampling rate up to at leat 800Hz */
ioctl(fd, GYROIOCSSAMPLERATE, 800);
/* set the driver to poll at 800Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 800);
#endif
warnx("using system gyro"); warnx("using system gyro");
close(fd); close(fd);
} }
@ -1360,6 +1417,9 @@ Sensors::task_main()
/* rate limit vehicle status updates to 5Hz */ /* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200); orb_set_interval(_vstatus_sub, 200);
/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
orb_set_interval(_gyro_sub, 4);
/* /*
* do advertisements * do advertisements
*/ */
@ -1395,7 +1455,7 @@ Sensors::task_main()
while (!_task_should_exit) { while (!_task_should_exit) {
/* wait for up to 500ms for data */ /* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */ /* timed out - periodic check for _task_should_exit, etc. */