diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index c13035f363..48f914a448 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -39,6 +39,7 @@ #include #include #include +#include using namespace matrix; @@ -376,5 +377,13 @@ bool FlightTaskManualAltitude::update() _updateSetpoints(); _constraints.want_takeoff = _checkTakeoff(); + static uint64_t delay_1s = hrt_absolute_time(); + static uint64_t delt_time; + if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL) + { + printf("update,delt:%dms \n",(hrt_absolute_time() - delt_time)/(1 * 1000ULL) ); + delay_1s = hrt_absolute_time(); + } + delt_time = hrt_absolute_time(); return ret; } diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index b668559a8e..e8d06a9736 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -159,7 +159,7 @@ void LoggedTopics::add_default_topics() // log all raw sensors at minimal rate (at least 1 Hz) add_topic_multi("battery_status", 200, 2); add_topic_multi("differential_pressure", 1000, 2); - add_topic_multi("distance_sensor", 1000); + add_topic_multi("distance_sensor", 100); add_topic_multi("optical_flow", 1000, 1); add_topic_multi("sensor_accel", 1000, 4); add_topic_multi("sensor_baro", 1000, 4); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c06c3d0633..661017dc0e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1521,7 +1521,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); configure_stream_local("COLLISION", unlimited_rate); - configure_stream_local("DISTANCE_SENSOR", 0.5f); + // configure_stream_local("DISTANCE_SENSOR", 0.5f); + configure_stream_local("DISTANCE_SENSOR", 10.0f); configure_stream_local("ESC_INFO", 1.0f); configure_stream_local("ESC_STATUS", 1.0f); configure_stream_local("ESTIMATOR_STATUS", 0.5f);