From a1b17326a4d9ca86f3c87f0047c48362a2e45fa6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 25 Aug 2012 18:27:34 -0700 Subject: Fix sensor subscriptions. Default to publishing. Make the sensors command and the sensors task visibly distinct in a task listing. Correctly check for bma180/l3gd20 in use. --- apps/sensors/sensors.cpp | 80 ++++++++++++++++++++++++++++-------------------- 1 file changed, 46 insertions(+), 34 deletions(-) (limited to 'apps') diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 0fc88f9f6..8b65708b1 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -247,14 +247,14 @@ Sensors::Sensors() : _task_should_exit(false), _sensors_task(-1), _hil_enabled(false), - _publishing(false), + _publishing(true), /* subscriptions */ - _gyro_sub(orb_subscribe(ORB_ID(sensor_gyro))), - _accel_sub(orb_subscribe(ORB_ID(sensor_accel))), - _mag_sub(orb_subscribe(ORB_ID(sensor_mag))), - _baro_sub(orb_subscribe(ORB_ID(sensor_baro))), - _vstatus_sub(orb_subscribe(ORB_ID(vehicle_status))), + _gyro_sub(-1), + _accel_sub(-1), + _mag_sub(-1), + _baro_sub(-1), + _vstatus_sub(-1), /* publications */ _sensor_pub(-1), @@ -262,7 +262,7 @@ Sensors::Sensors() : _rc_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "sensor update")) + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) { /* min values */ _parameter_handles.min[0] = param_find("RC1_MIN"); @@ -436,11 +436,11 @@ Sensors::accel_init() } else { /* set the accel internal sampling rate up to at leat 500Hz */ if (OK != ioctl(fd, ACCELIOCSSAMPLERATE, 500)) - warn("failed to set minimum 500Hz sample rate for accel"); + warn("WARNING: failed to set minimum 500Hz sample rate for accel"); /* set the driver to poll at 500Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 500)) - warn("failed to set 500Hz poll rate for accel"); + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 50/*0*/)) + warn("WARNING: failed to set 500Hz poll rate for accel"); warnx("using system accel"); close(fd); @@ -471,11 +471,11 @@ Sensors::gyro_init() } else { /* set the gyro internal sampling rate up to at leat 500Hz */ if (OK != ioctl(fd, GYROIOCSSAMPLERATE, 500)) - warn("failed to set minimum 500Hz sample rate for gyro"); + warn("WARNING: failed to set minimum 500Hz sample rate for gyro"); /* set the driver to poll at 500Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 500)) - warn("failed to set 500Hz poll rate for gyro"); + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 50/*0*/)) + warn("WARNING: failed to set 500Hz poll rate for gyro"); warnx("using system gyro"); close(fd); @@ -495,11 +495,11 @@ Sensors::mag_init() /* set the mag internal poll rate to at least 150Hz */ if (OK != ioctl(fd, MAGIOCSSAMPLERATE, 150)) - warn("failed to set minimum 150Hz sample rate for mag"); + warn("WARNING: failed to set minimum 150Hz sample rate for mag"); /* set the driver to poll at 150Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 150)) - warn("failed to set 150Hz poll rate for mag"); + warn("WARNING: failed to set 150Hz poll rate for mag"); close(fd); } @@ -517,7 +517,7 @@ Sensors::baro_init() /* set the driver to poll at 150Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 150)) - warn("failed to set 150Hz poll rate for baro"); + warn("WARNING: failed to set 150Hz poll rate for baro"); close(fd); } @@ -538,7 +538,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) { struct accel_report accel_report; - if (_fd_bma180) { + if (_fd_bma180 >= 0) { /* do ORB emulation for BMA180 */ int16_t buf[3]; @@ -574,7 +574,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) { struct gyro_report gyro_report; - if (_fd_gyro_l3gd20) { + if (_fd_gyro_l3gd20 >= 0) { /* do ORB emulation for L3GD20 */ int16_t buf[3]; @@ -763,17 +763,6 @@ Sensors::task_main_trampoline(int argc, char *argv[]) void Sensors::task_main() { - /* inform about start */ - printf("[sensors] Initializing..\n"); - fflush(stdout); - - /* start individual sensors */ - accel_init(); - gyro_init(); - mag_init(); - baro_init(); - adc_init(); - #pragma pack(push,1) struct adc_msg4_s { uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ @@ -790,6 +779,32 @@ Sensors::task_main() struct adc_msg4_s buf_adc; size_t adc_readsize = 1 * sizeof(struct adc_msg4_s); + /* inform about start */ + printf("[sensors] Initializing..\n"); + fflush(stdout); + + /* start individual sensors */ + accel_init(); + gyro_init(); + mag_init(); + baro_init(); + adc_init(); + + /* + * do subscriptions + */ + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vstatus_sub, 200); + + /* + * do advertisements + */ struct sensor_combined_s raw; raw.timestamp = hrt_absolute_time(); raw.battery_voltage_v = BAT_VOL_INITIAL; @@ -827,9 +842,6 @@ Sensors::task_main() _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); } - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); - /* wakeup sources */ struct pollfd fds[1]; @@ -848,7 +860,7 @@ Sensors::task_main() /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { - warn("poll error"); + warn("poll error %d, %d", pret, errno); continue; } @@ -914,7 +926,7 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = task_create("sensors", + _sensors_task = task_create("sensor_task", SCHED_PRIORITY_MAX - 5, 4096, (main_t)&Sensors::task_main_trampoline, -- cgit v1.2.3