From 5c402fa3989e420b65c888966a9fca79e3ceaf30 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Sep 2012 07:54:21 +1000 Subject: [PATCH] APM: allow RawSensors stream rate to be saved if <= 5 this makes it possible to configure all stream rates via a parameter file --- ArduPlane/GCS_Mavlink.pde | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 15f187ab93..1cbea32140 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -968,9 +968,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_DATA_STREAM_RAW_SENSORS: - streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly - // we will not continue to broadcast raw sensor data at 50Hz. + if (freq <= 5) { + streamRateRawSensors.set_and_save_ifchanged(freq); + } else { + // We do not set and save this one so that if HIL is shut down incorrectly + // we will not continue to broadcast raw sensor data at 50Hz. + streamRateRawSensors = freq; + } break; + case MAV_DATA_STREAM_EXTENDED_STATUS: streamRateExtendedStatus.set_and_save_ifchanged(freq); break; @@ -983,10 +989,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) streamRateRawController.set_and_save_ifchanged(freq); break; - //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: - // streamRateRawSensorFusion.set_and_save(freq); - // break; - case MAV_DATA_STREAM_POSITION: streamRatePosition.set_and_save_ifchanged(freq); break;