From a740a1b091a2c68d0b564309bb8b0df0e8f4ad3f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=82=A3=E4=B8=AAZeng?= Date: Sun, 3 Apr 2022 22:16:34 +0800 Subject: [PATCH] =?UTF-8?q?=E8=B0=83=E6=95=B4Mavlink=E5=92=8C=E6=97=A5?= =?UTF-8?q?=E5=BF=97=E9=A2=91=E7=8E=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp | 3 ++- src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.h | 2 +- src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c_main.cpp | 3 +-- src/modules/commander/state_machine_helper.cpp | 2 +- src/modules/logger/logged_topics.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 3 ++- 6 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp index dae4be8744..97ecfef95a 100644 --- a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp +++ b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp @@ -54,7 +54,8 @@ TFmini_i2c::TFmini_i2c(I2CSPIBusOption bus_option, const int bus, const uint8_t { _px4_rangefinder.set_min_distance(TFMINI_MIN_DISTANCE); _px4_rangefinder.set_max_distance(TFMINI_MAX_DISTANCE); - _px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian + _px4_rangefinder.set_fov(math::radians(1.15f)); + // up the retries since the device misses the first measure attempts _retries = 3; diff --git a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.h b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.h index 06eaf917ec..6d20936523 100644 --- a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.h +++ b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.h @@ -86,7 +86,7 @@ static constexpr int TFMINI_PEAK_STRENGTH_LOW = 135; /* Minimum peak str static constexpr int TFMINI_PEAK_STRENGTH_HIGH = 234; /* Max peak strength raw value */ -static constexpr float TFMINI_MIN_DISTANCE{0.05f}; +static constexpr float TFMINI_MIN_DISTANCE{0.2f}; static constexpr float TFMINI_MAX_DISTANCE{12.00f}; static constexpr float TFMINI_MAX_DISTANCE_V2{35.00f}; diff --git a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c_main.cpp b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c_main.cpp index 82ca275a8d..9942115d7d 100644 --- a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c_main.cpp +++ b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c_main.cpp @@ -138,8 +138,7 @@ extern "C" __EXPORT int tfmini_i2c_main(int argc, char *argv[]) } if (!strcmp(verb, "test")) { - static float dist; - dist += 1.1; + double dist= 1.1; printf("test dist:%.2f\n",dist); return 0; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6cadb0d1b0..83366d162d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -280,7 +280,7 @@ transition_result_t arming_state_transition(vehicle_status_s &status, const safe return ret; } - +// 判断遥控器是否切换成功 transition_result_t main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state, const vehicle_status_flags_s &status_flags, commander_state_s &internal_state) 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);