From fc6436338c361dad0866a2d29a1d5d3b8e3de2ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=82=A3=E4=B8=AAZeng?= Date: Wed, 6 Apr 2022 23:46:40 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=9C=B0=E5=9D=80=E8=B0=83?= =?UTF-8?q?=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../distance_sensor/tfmini_i2c/tfmini_i2c.cpp | 59 ++++++++++++------- .../FlightTaskManualAltitude.cpp | 8 --- v5_build.sh | 2 +- 3 files changed, 38 insertions(+), 31 deletions(-) diff --git a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp index 7cfb38fc87..f473c1b010 100644 --- a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp +++ b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp @@ -69,18 +69,13 @@ using namespace time_literals; /* Configuration Constants */ -#define TFMINI_I2C_BASE_ADDR 0x10 // 7-bit address is 0x70 = 112. 8-bit address is 0xE0 = 224. -#define TFMINI_I2C_MIN_ADDR 0x0f // 7-bit address is 0x5A = 90. 8-bit address is 0xB4 = 180. +#define TFMINI_I2C_BASE_ADDR 0x10 +#define TFMINI_I2C_MAX_ADDR 0x1A #define TFMINI_I2C_BUS_SPEED 100000 // 100kHz bus speed. -/* TF_I2Cxx Registers addresses */ -#define TFMINI_I2C_TAKE_RANGE_REG 0x51 // Measure range Register. -#define TFMINI_I2C_SET_ADDRESS_1 0xAA // Change address 1 Register. -#define TFMINI_I2C_SET_ADDRESS_2 0xA5 // Change address 2 Register. - /* Device limits */ #define TFMINI_I2C_MIN_DISTANCE (0.10f) -#define TFMINI_I2C_MAX_DISTANCE (8.00f) +#define TFMINI_I2C_MAX_DISTANCE (12.00f) #define TFMINI_I2C_MEASURE_INTERVAL 100_ms // 60ms minimum for one sonar. #define TFMINI_I2C_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100_ms // 30ms minimum between each sonar measurement (watch out for interference!). @@ -211,7 +206,7 @@ TFMINI_I2C::collect() static uint64_t last_us ; perf_begin(_sample_perf); - uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 }; + uint8_t CMD_READ_MEASUREMENT[] = {0x5A,0x05,0x00,0x01,0x60}; static uint8_t raw_data[11]; // Increment i2c adress to next sensor. @@ -227,9 +222,15 @@ TFMINI_I2C::collect() if (ret_val < 0) { PX4_ERR("sensor %i read failed, address: 0x%02X", _sensor_index, get_device_address()); - perf_count(_comms_error); - perf_end(_sample_perf); - return ret_val; + // perf_count(_comms_error); + // perf_end(_sample_perf); + if (measure() != PX4_OK) { + PX4_INFO("sensor %i measurement error, address 0x%02X", _sensor_index, get_device_address()); + perf_count(_comms_error); + perf_end(_sample_perf); + return ret_val; + } + // return ret_val; } ///////////////////// @@ -269,6 +270,10 @@ TFMINI_I2C::collect() /////////////////////// uint16_t distance_cm = distance; float distance_m = static_cast(distance_cm) * 1e-2f; + if (distance_m > 13.0f) + { + distance_m = 13.0f; + } distance_sensor_s report; report.current_distance = distance_m; @@ -287,12 +292,12 @@ TFMINI_I2C::collect() orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id); // Begin the next measurement. - if (measure() != PX4_OK) { - PX4_INFO("sensor %i measurement error, address 0x%02X", _sensor_index, get_device_address()); - perf_count(_comms_error); - perf_end(_sample_perf); - return ret_val; - } + // if (measure() != PX4_OK) { + // PX4_INFO("sensor %i measurement error, address 0x%02X", _sensor_index, get_device_address()); + // perf_count(_comms_error); + // perf_end(_sample_perf); + // return ret_val; + // } perf_end(_sample_perf); return PX4_OK; @@ -359,8 +364,8 @@ TFMINI_I2C::init() } // Check for connected rangefinders on each i2c port by decrementing from the base address, - // (TFMINI_I2C_BASE_ADDR = 112, TFMINI_I2C_MIN_ADDR = 90). - for (uint8_t address = TFMINI_I2C_BASE_ADDR; address > TFMINI_I2C_MIN_ADDR ; address--) { + // (TFMINI_I2C_BASE_ADDR = 0x10, TFMINI_I2C_MAX_ADDR = 0x1a). + for (uint8_t address = TFMINI_I2C_BASE_ADDR; address < TFMINI_I2C_MAX_ADDR ; address++) { set_device_address(address); if (measure() == PX4_OK) { @@ -376,6 +381,8 @@ TFMINI_I2C::init() } px4_usleep(_measure_interval); + }else{ + PX4_INFO("\n tf err sensor %i at address 0x%02X added\n", _sensor_count, address); } } @@ -464,13 +471,21 @@ TFMINI_I2C::set_address(const uint8_t address) PX4_INFO("requested address: %u", address); - uint8_t shifted_address = address ; + uint8_t shifted_address = address; uint8_t cmd[5] = {0x5A, 0x05, 0x0B, shifted_address,0}; - cmd[4] = 0x5A + 0x05 + 0x0B + shifted_address; // 最后一位和校验 + uint8_t ck_sum = 0x5A + 0x05 + 0x0B + shifted_address; // 最后一位和校验 + cmd[4] = ck_sum; if (transfer(cmd, sizeof(cmd), nullptr, 0) != PX4_OK) { PX4_INFO("could not set the address"); } + printf("{%d}set_address arr\n",__LINE__); + for (uint8_t i = 0; i < 5; i++) + { + printf("%02x ",cmd[i]); + } + printf("\n"); + set_device_address(address); PX4_INFO("device address: %u", get_device_address()); return PX4_OK; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 48f914a448..807b5bfc52 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -377,13 +377,5 @@ 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/v5_build.sh b/v5_build.sh index 5f4ac9a68d..d8c66d4444 100755 --- a/v5_build.sh +++ b/v5_build.sh @@ -1,6 +1,6 @@ date -R starttime=`date +'%Y-%m-%d %H:%M:%S'` -make px4_fmu-v5_default +make px4_fmu-v5_default upload endtime=`date +'%Y-%m-%d %H:%M:%S'` date -R start_seconds=$(date --date="$starttime" +%s);