From 866e07c3263b74e9639f17f5e3fa4773d2226fd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Sep 2015 17:21:31 +0200 Subject: [PATCH 1/2] MAVLink app: Configure USB link correctly --- src/modules/mavlink/mavlink_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 90729fb138..7b2ef9b129 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1693,6 +1693,10 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_CONFIG: // Enable a number of interesting streams we want via USB + configure_stream("SYS_STATUS", 1.0f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + configure_stream("GLOBAL_POSITION_INT", 10.0f); + configure_stream("ATTITUDE_TARGET", 8.0f); configure_stream("PARAM_VALUE", 300.0f); configure_stream("MISSION_ITEM", 50.0f); configure_stream("NAMED_VALUE_FLOAT", 10.0f); @@ -1707,7 +1711,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); configure_stream("MANUAL_CONTROL", 5.0f); - configure_stream("HIGHRES_IMU", 100.0f); + configure_stream("HIGHRES_IMU", 50.0f); configure_stream("GPS_RAW_INT", 20.0f); configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("VTOL_STATE", 2.0f); From 5c52efece4b988ca8f13fc12c3975e32705f927a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Sep 2015 17:33:37 +0200 Subject: [PATCH 2/2] Sensors app: Rate limit new sensor discovery --- src/modules/sensors/sensors.cpp | 54 +++++++++++++++++++-------------- 1 file changed, 31 insertions(+), 23 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 86df382df8..ce6f7939ad 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -212,7 +212,7 @@ private: unsigned _accel_count; /**< raw accel data count */ unsigned _mag_count; /**< raw mag data count */ unsigned _baro_count; /**< raw baro data count */ - + int _rc_sub; /**< raw rc channels data subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ @@ -1988,7 +1988,7 @@ Sensors::task_main() } while (0); if (ret) { - warnx("Sensor initialization failed"); + warnx("sensor initialization failed"); _sensors_task = -1; if (_fd_adc >=0) { px4_close(_fd_adc); @@ -2068,6 +2068,8 @@ Sensors::task_main() raw.timestamp = 0; + uint64_t _last_config_update = hrt_absolute_time(); + while (!_task_should_exit) { /* wait for up to 50ms for data */ @@ -2077,7 +2079,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 %d, %d", pret, errno); + warnx("poll error %d, %d", pret, errno); continue; } @@ -2086,21 +2088,6 @@ Sensors::task_main() /* check vehicle status for changes to publication state */ vehicle_control_mode_poll(); - /* keep adding sensors as long as we are not armed */ - if (!_armed) { - _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_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]); - - _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_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]); - } - /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ gyro_poll(raw); @@ -2129,11 +2116,32 @@ Sensors::task_main() orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); } - /* check parameters for updates */ - parameter_update_poll(); + /* keep adding sensors as long as we are not armed, + * when not adding sensors poll for param updates + */ + if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) { + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], + &raw.gyro_priority[0], &raw.gyro_errcount[0]); - /* check rc parameter map for updates */ - rc_parameter_map_poll(); + _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_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]); + + _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], + &raw.baro_priority[0], &raw.baro_errcount[0]); + + _last_config_update = hrt_absolute_time(); + + } else { + + /* check parameters for updates */ + parameter_update_poll(); + + /* check rc parameter map for updates */ + rc_parameter_map_poll(); + } /* Look for new r/c input data */ rc_poll(); @@ -2152,7 +2160,7 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = px4_task_spawn_cmd("sensors_task", + _sensors_task = px4_task_spawn_cmd("sensors", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000,