Compare commits
42 Commits
| Author | SHA1 | Date |
|---|---|---|
 那个Zeng
|
c0cc1ba6d0 | 3 years ago |
|
zbr3550
|
d76bd9645e | 3 years ago |
|
zbr3550
|
ea00db03ee | 3 years ago |
|
孤帆远影Faraway
|
05f22821ef | 3 years ago |
honglang
|
55374fe589 | 3 years ago |
Daniel Agar
|
2e8918da66 | 3 years ago |
|
Daniel Agar
|
5def17b1af | 3 years ago |
|
Daniel Agar
|
1daf0654ff | 3 years ago |
|
Daniel Agar
|
6e3a2314cb | 3 years ago |
Sander Swart
|
6121b287f1 | 3 years ago |
|
Sander Swart
|
c0980bd273 | 3 years ago |
|
Sander Swart
|
42d14e6072 | 3 years ago |
|
Daniel Agar
|
ba0b512f0d | 3 years ago |
|
Daniel Agar
|
7476a2953e | 3 years ago |
Lorenz Meier
|
15e6ca9e1e | 3 years ago |
|
Daniel Agar
|
b9e7063237 | 3 years ago |
|
Daniel Agar
|
0a6045367f | 3 years ago |
|
Daniel Agar
|
3f88a6d0bf | 3 years ago |
Jukka Laitinen
|
d1c09ec358 | 3 years ago |
|
Daniel Agar
|
ca744626cd | 3 years ago |
|
Daniel Agar
|
ea577f15b9 | 3 years ago |
|
Daniel Agar
|
d9f3c820ab | 4 years ago |
David Sidrane
|
8e57a62c9d | 4 years ago |
|
David Sidrane
|
b961f6a1e1 | 4 years ago |
|
David Sidrane
|
09140c01bf | 4 years ago |
|
David Sidrane
|
822ae46235 | 4 years ago |
|
David Sidrane
|
f70381dfdd | 4 years ago |
|
David Sidrane
|
84066f574d | 4 years ago |
|
David Sidrane
|
c8426da50d | 4 years ago |
|
David Sidrane
|
6bab917a3d | 4 years ago |
|
David Sidrane
|
531301e176 | 4 years ago |
|
David Sidrane
|
04f4157828 | 4 years ago |
|
Daniel Agar
|
c5f82ed838 | 4 years ago |
|
Daniel Agar
|
99501b4c38 | 4 years ago |
|
Daniel Agar
|
9e4a04f58a | 4 years ago |
|
Daniel Agar
|
1682fd5671 | 4 years ago |
bresch
|
a8572f0fdd | 4 years ago |
|
Daniel Agar
|
a299a3bbd0 | 4 years ago |
Julian Oes
|
349dd63072 | 4 years ago |
|
David Sidrane
|
6b51c6390a | 4 years ago |
|
David Sidrane
|
4f7909ee8e | 4 years ago |
|
David Sidrane
|
cbb48f9af3 | 4 years ago |
58 changed files with 1494 additions and 326 deletions
@ -1,72 +1,72 @@
@@ -1,72 +1,72 @@
|
||||
[submodule "mavlink/include/mavlink/v2.0"] |
||||
path = mavlink/include/mavlink/v2.0 |
||||
url = https://github.com/mavlink/c_library_v2.git |
||||
url = https://gitee.com/nzeg/c_library_v2.git |
||||
branch = master |
||||
[submodule "src/drivers/uavcan/libuavcan"] |
||||
path = src/drivers/uavcan/libuavcan |
||||
url = https://github.com/PX4/libuavcan.git |
||||
url = https://gitee.com/nzeg/libuavcan_px4.git |
||||
branch = px4 |
||||
[submodule "Tools/jMAVSim"] |
||||
path = Tools/jMAVSim |
||||
url = https://github.com/PX4/jMAVSim.git |
||||
url = https://gitee.com/nzeg/jMAVSim.git |
||||
branch = master |
||||
[submodule "Tools/sitl_gazebo"] |
||||
path = Tools/sitl_gazebo |
||||
url = https://github.com/PX4/PX4-SITL_gazebo.git |
||||
url = https://gitee.com/nzeg/PX4-SITL_gazebo.git |
||||
branch = master |
||||
[submodule "src/lib/matrix"] |
||||
path = src/lib/matrix |
||||
url = https://github.com/PX4/PX4-Matrix.git |
||||
url = https://gitee.com/nzeg/PX4-Matrix.git |
||||
branch = master |
||||
[submodule "src/lib/ecl"] |
||||
path = src/lib/ecl |
||||
url = https://github.com/PX4/PX4-ECL.git |
||||
url = https://gitee.com/nzeg/PX4-ECL.git |
||||
branch = master |
||||
[submodule "boards/atlflight/cmake_hexagon"] |
||||
path = boards/atlflight/cmake_hexagon |
||||
url = https://github.com/PX4/cmake_hexagon.git |
||||
url = https://gitee.com/nzeg/cmake_hexagon.git |
||||
branch = px4 |
||||
[submodule "src/drivers/gps/devices"] |
||||
path = src/drivers/gps/devices |
||||
url = https://github.com/PX4/PX4-GPSDrivers.git |
||||
url = https://gitee.com/nzeg/PX4-GPSDrivers.git |
||||
branch = master |
||||
[submodule "src/modules/micrortps_bridge/micro-CDR"] |
||||
path = src/modules/micrortps_bridge/micro-CDR |
||||
url = https://github.com/PX4/Micro-CDR.git |
||||
url = https://gitee.com/nzeg/Micro-CDR.git |
||||
branch = master |
||||
[submodule "platforms/nuttx/NuttX/nuttx"] |
||||
path = platforms/nuttx/NuttX/nuttx |
||||
url = https://github.com/PX4/NuttX.git |
||||
url = https://gitee.com/nzeg/NuttX.git |
||||
branch = px4_firmware_nuttx-10.0.0+ |
||||
[submodule "platforms/nuttx/NuttX/apps"] |
||||
path = platforms/nuttx/NuttX/apps |
||||
url = https://github.com/PX4/NuttX-apps.git |
||||
url = https://gitee.com/nzeg/NuttX-apps.git |
||||
branch = px4_firmware_nuttx-10.0.0+ |
||||
[submodule "platforms/qurt/dspal"] |
||||
path = platforms/qurt/dspal |
||||
url = https://github.com/ATLFlight/dspal.git |
||||
url = https://gitee.com/nzeg/dspal.git |
||||
[submodule "Tools/flightgear_bridge"] |
||||
path = Tools/flightgear_bridge |
||||
url = https://github.com/PX4/PX4-FlightGear-Bridge.git |
||||
url = https://gitee.com/nzeg/PX4-FlightGear-Bridge.git |
||||
[submodule "Tools/jsbsim_bridge"] |
||||
path = Tools/jsbsim_bridge |
||||
url = https://github.com/PX4/px4-jsbsim-bridge.git |
||||
url = https://gitee.com/nzeg/px4-jsbsim-bridge.git |
||||
[submodule "src/drivers/uavcan_v1/libcanard"] |
||||
path = src/drivers/uavcan_v1/libcanard |
||||
url = https://github.com/UAVCAN/libcanard.git |
||||
url = https://gitee.com/nzeg/libcanard_px4.git |
||||
[submodule "src/drivers/uavcan_v1/public_regulated_data_types"] |
||||
path = src/drivers/uavcan_v1/public_regulated_data_types |
||||
url = https://github.com/UAVCAN/public_regulated_data_types.git |
||||
url = https://gitee.com/nzeg/public_regulated_data_types.git |
||||
[submodule "src/drivers/uavcannode_gps_demo/public_regulated_data_types"] |
||||
path = src/drivers/uavcannode_gps_demo/public_regulated_data_types |
||||
url = https://github.com/UAVCAN/public_regulated_data_types.git |
||||
url = https://gitee.com/nzeg/public_regulated_data_types.git |
||||
[submodule "src/drivers/uavcannode_gps_demo/libcanard"] |
||||
path = src/drivers/uavcannode_gps_demo/libcanard |
||||
url = https://github.com/UAVCAN/libcanard.git |
||||
url = https://gitee.com/nzeg/libcanard_px4.git |
||||
[submodule "src/drivers/uavcan_v1/legacy_data_types"] |
||||
path = src/drivers/uavcan_v1/legacy_data_types |
||||
url = https://github.com/PX4/public_regulated_data_types.git |
||||
url = https://gitee.com/nzeg/public_regulated_data_types_px42.git |
||||
branch = legacy |
||||
[submodule "src/lib/crypto/monocypher"] |
||||
path = src/lib/crypto/monocypher |
||||
url = https://github.com/PX4/Monocypher.git |
||||
url = https://gitee.com/nzeg/Monocypher.git |
||||
|
||||
@ -0,0 +1,8 @@
@@ -0,0 +1,8 @@
|
||||
date -R |
||||
starttime=`date +'%Y-%m-%d %H:%M:%S'` |
||||
make px4_fmu-v2_default |
||||
endtime=`date +'%Y-%m-%d %H:%M:%S'` |
||||
date -R |
||||
start_seconds=$(date --date="$starttime" +%s); |
||||
end_seconds=$(date --date="$endtime" +%s); |
||||
echo "本次运行时间： "$((end_seconds-start_seconds))"s" |
||||
@ -0,0 +1,8 @@
@@ -0,0 +1,8 @@
|
||||
date -R |
||||
starttime=`date +'%Y-%m-%d %H:%M:%S'` |
||||
make px4_fmu-v3_default |
||||
endtime=`date +'%Y-%m-%d %H:%M:%S'` |
||||
date -R |
||||
start_seconds=$(date --date="$starttime" +%s); |
||||
end_seconds=$(date --date="$endtime" +%s); |
||||
echo "本次运行时间： "$((end_seconds-start_seconds))"s" |
||||
@ -1 +1 @@
@@ -1 +1 @@
|
||||
Subproject commit 76bb42f3ebd902102e844084b564274bf215ec9f |
||||
Subproject commit bf660cba2af81f055002b3817c87b1f63a78fd09 |
||||
@ -0,0 +1,43 @@
@@ -0,0 +1,43 @@
|
||||
############################################################################ |
||||
# |
||||
# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. |
||||
# |
||||
# Redistribution and use in source and binary forms, with or without |
||||
# modification, are permitted provided that the following conditions |
||||
# are met: |
||||
# |
||||
# 1. Redistributions of source code must retain the above copyright |
||||
# notice, this list of conditions and the following disclaimer. |
||||
# 2. Redistributions in binary form must reproduce the above copyright |
||||
# notice, this list of conditions and the following disclaimer in |
||||
# the documentation and/or other materials provided with the |
||||
# distribution. |
||||
# 3. Neither the name PX4 nor the names of its contributors may be |
||||
# used to endorse or promote products derived from this software |
||||
# without specific prior written permission. |
||||
# |
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
# POSSIBILITY OF SUCH DAMAGE. |
||||
# |
||||
############################################################################ |
||||
px4_add_module( |
||||
MODULE drivers__tfmini_i2c |
||||
MAIN tfmini_i2c |
||||
COMPILE_FLAGS |
||||
-Wno-cast-align # TODO: fix and enable |
||||
SRCS |
||||
tfmini_i2c.cpp |
||||
tfmini_i2c_main.cpp |
||||
DEPENDS |
||||
drivers_rangefinder |
||||
) |
||||
@ -0,0 +1,646 @@
@@ -0,0 +1,646 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2014-2019, 2021 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
|
||||
/**
|
||||
* @file TFmini_i2c.cpp |
||||
* @author Allyson Kreft |
||||
* |
||||
* Driver for the PulsedLight Lidar-Lite range finders connected via I2C. |
||||
*/ |
||||
|
||||
#include "tfmini_i2c.h" |
||||
#include <px4_platform_common/getopt.h> |
||||
#include <px4_platform_common/module.h> |
||||
|
||||
#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) |
||||
|
||||
TFmini_i2c::TFmini_i2c(I2CSPIBusOption bus_option, const int bus, const uint8_t orientation, int bus_frequency, |
||||
const int address) : |
||||
I2C(DRV_DIST_DEVTYPE_TFMINI_I2C, MODULE_NAME, bus, address, bus_frequency), |
||||
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), |
||||
_px4_rangefinder(get_device_id(), orientation) |
||||
{ |
||||
_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
|
||||
// up the retries since the device misses the first measure attempts
|
||||
_retries = 3; |
||||
|
||||
_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_TFMINI_I2C); /// TODO
|
||||
} |
||||
|
||||
TFmini_i2c::~TFmini_i2c() |
||||
{ |
||||
perf_free(_sample_perf); |
||||
perf_free(_comms_errors); |
||||
perf_free(_sensor_resets); |
||||
perf_free(_sensor_zero_resets); |
||||
} |
||||
|
||||
int |
||||
TFmini_i2c::init() |
||||
{ |
||||
printf("TFmini_i2c: init() \n"); |
||||
// Perform I2C init (and probe) first.
|
||||
if (I2C::init() != PX4_OK) { |
||||
return PX4_ERROR; |
||||
} |
||||
int32_t hw_model = 1; // only one model so far...
|
||||
|
||||
switch (hw_model) { |
||||
case 1: // TFMINI (12m, 100 Hz)
|
||||
// Note:
|
||||
// Sensor specification shows 0.3m as minimum, but in practice
|
||||
// 0.3 is too close to minimum so chattering of invalid sensor decision
|
||||
// is happening sometimes. this cause EKF to believe inconsistent range readings.
|
||||
// So we set 0.4 as valid minimum.
|
||||
_px4_rangefinder.set_min_distance(0.4f); |
||||
_px4_rangefinder.set_max_distance(12.0f); |
||||
_px4_rangefinder.set_fov(math::radians(1.15f)); |
||||
|
||||
break; |
||||
|
||||
default: |
||||
PX4_ERR("invalid HW model %" PRId32 ".", hw_model); |
||||
return -1; |
||||
} |
||||
|
||||
|
||||
return PX4_OK; |
||||
} |
||||
|
||||
void |
||||
TFmini_i2c::print_status() |
||||
{ |
||||
I2CSPIDriverBase::print_status(); |
||||
perf_print_counter(_sample_perf); |
||||
perf_print_counter(_comms_errors); |
||||
perf_print_counter(_sensor_resets); |
||||
perf_print_counter(_sensor_zero_resets); |
||||
printf("poll interval: %" PRIu32 "\n", get_measure_interval()); |
||||
} |
||||
|
||||
int |
||||
TFmini_i2c::read_reg(const uint8_t reg, uint8_t &val) |
||||
{ |
||||
return lidar_transfer(®, 1, &val, 1); |
||||
} |
||||
|
||||
int |
||||
TFmini_i2c::write_reg(const uint8_t reg, const uint8_t &val) |
||||
{ |
||||
const uint8_t cmd[2] = { reg, val }; |
||||
return transfer(&cmd[0], 2, nullptr, 0); |
||||
} |
||||
|
||||
int |
||||
TFmini_i2c::lidar_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len) |
||||
{ |
||||
if (send != nullptr && send_len > 0) { |
||||
int ret = transfer(send, send_len, nullptr, 0); |
||||
|
||||
if (ret != PX4_OK) { |
||||
return ret; |
||||
} |
||||
} |
||||
|
||||
if (recv != nullptr && recv_len > 0) { |
||||
return transfer(nullptr, 0, recv, recv_len); |
||||
} |
||||
|
||||
return PX4_ERROR; |
||||
} |
||||
|
||||
|
||||
int |
||||
TFmini_i2c::check_checksum(uint8_t *arr, int pkt_len) |
||||
{ |
||||
uint8_t checksum = 0; |
||||
int i; |
||||
|
||||
/* sum them all except the last (the checksum) */ |
||||
for (i = 0; i < pkt_len - 1; i++) { |
||||
checksum += arr[i]; |
||||
} |
||||
|
||||
return checksum == arr[pkt_len - 1]; |
||||
} |
||||
|
||||
int |
||||
TFmini_i2c::probe() |
||||
{ |
||||
|
||||
printf("TFmini_i2c: probe() \n"); |
||||
const uint8_t CMD_FW_VERSION[] = { 0x5A, 0x04, 0x01, 0x5F }; |
||||
const uint8_t CMD_SYSTEM_RESET[] = { 0x5A, 0x04, 0x04, 0x62 }; |
||||
const uint8_t CMD_OUTPUT_FORMAT_CM[] = { 0x5A, 0x05, 0x05, 0x01, 0x65 }; |
||||
const uint8_t CMD_ENABLE_DATA_OUTPUT[] = { 0x5A, 0x05, 0x07, 0x01, 0x67 }; |
||||
const uint8_t CMD_FRAME_RATE_250HZ[] = { 0x5A, 0x06, 0x03, 0xFA, 0x00, 0x5D }; |
||||
const uint8_t CMD_SAVE_SETTINGS[] = { 0x5A, 0x04, 0x11, 0x6F }; |
||||
const uint8_t *cmds[] = { |
||||
CMD_OUTPUT_FORMAT_CM, |
||||
CMD_FRAME_RATE_250HZ, |
||||
CMD_ENABLE_DATA_OUTPUT, |
||||
CMD_SAVE_SETTINGS, |
||||
}; |
||||
uint8_t val[12], i; |
||||
int ret; |
||||
|
||||
// cope with both old and new I2C bus address
|
||||
const uint8_t addresses[2] = { TFMINI_BASEADDR, TFMINI_BASEADDR_OLD }; |
||||
|
||||
// more retries for detection
|
||||
_retries = 10; |
||||
|
||||
for (uint8_t i = 0; i < sizeof(addresses); i++) { |
||||
|
||||
set_device_address(addresses[i]); |
||||
|
||||
|
||||
ret = lidar_transfer(CMD_FW_VERSION, sizeof(CMD_FW_VERSION), nullptr, 0); |
||||
if (ret != PX4_OK) { |
||||
goto fail; |
||||
} |
||||
px4_usleep(100_ms); |
||||
ret = lidar_transfer(nullptr, 0, val, 7); |
||||
if (!ret || val[0] != 0x5A || val[1] != 0x07 || val[2] != 0x01 || |
||||
!check_checksum(val, 7)) { |
||||
goto fail; |
||||
} |
||||
|
||||
if (val[5] * 10000 + val[4] * 100 + val[3] < 20003) { |
||||
PX4_INFO("TFMini: FW ver %u.%u.%u (need>=2.0.3)",(unsigned)val[5],(unsigned)val[4],(unsigned)val[3]); |
||||
goto fail; |
||||
} |
||||
PX4_INFO(": found fw version %u.%u.%u\n",val[5], val[4], val[3]); |
||||
|
||||
for (i = 0; i < ARRAY_SIZE(cmds); i++) { |
||||
ret = lidar_transfer(cmds[i], cmds[i][1], nullptr, 0); |
||||
if (!ret) { |
||||
PX4_INFO(": Unable to set configuration register %u\n",cmds[i][2]); |
||||
goto fail; |
||||
} |
||||
px4_usleep(100_ms); |
||||
} |
||||
lidar_transfer(CMD_SYSTEM_RESET, sizeof(CMD_SYSTEM_RESET), nullptr, 0); |
||||
return OK; |
||||
} |
||||
|
||||
fail: |
||||
// not found on any address
|
||||
return -EIO; |
||||
} |
||||
|
||||
int |
||||
TFmini_i2c::measure() |
||||
{ |
||||
if (_pause_measurements) { |
||||
// we are in print_registers() and need to avoid
|
||||
// acquisition to keep the I2C peripheral on the
|
||||
// sensor active
|
||||
return OK; |
||||
} |
||||
|
||||
// Send the command to begin a measurement.
|
||||
int ret = write_reg(TFMINI_MEASURE_REG, TFMINI_MSRREG_ACQUIRE); |
||||
|
||||
if (ret != PX4_OK) { |
||||
perf_count(_comms_errors); |
||||
PX4_DEBUG("i2c::transfer returned %d", ret); |
||||
|
||||
// if we are getting lots of I2C transfer errors try
|
||||
// resetting the sensor
|
||||
if (perf_event_count(_comms_errors) % 10 == 0) { |
||||
perf_count(_sensor_resets); |
||||
reset_sensor(); |
||||
} |
||||
|
||||
return ret; |
||||
} |
||||
|
||||
// remember when we sent the acquire so we can know when the
|
||||
// acquisition has timed out
|
||||
_acquire_time_usec = hrt_absolute_time(); |
||||
|
||||
return OK; |
||||
} |
||||
|
||||
int |
||||
TFmini_i2c::reset_sensor() |
||||
{ |
||||
px4_usleep(15_ms); |
||||
|
||||
int ret = write_reg(TFMINI_SIG_COUNT_VAL_REG, TFMINI_SIG_COUNT_VAL_MAX); |
||||
|
||||
if (ret != PX4_OK) { |
||||
return ret; |
||||
} |
||||
|
||||
px4_usleep(15_ms); |
||||
ret = write_reg(TFMINI_MEASURE_REG, TFMINI_MSRREG_RESET); |
||||
|
||||
|
||||
if (ret != PX4_OK) { |
||||
uint8_t sig_cnt; |
||||
|
||||
px4_usleep(15_ms); |
||||
ret = read_reg(TFMINI_SIG_COUNT_VAL_REG, sig_cnt); |
||||
|
||||
if ((ret != PX4_OK) || (sig_cnt != TFMINI_SIG_COUNT_VAL_DEFAULT)) { |
||||
PX4_INFO("Error: TFMINI reset failure. Exiting!\n"); |
||||
return ret; |
||||
|
||||
} |
||||
} |
||||
|
||||
// wait for sensor reset to complete
|
||||
px4_usleep(50_ms); |
||||
ret = write_reg(TFMINI_SIG_COUNT_VAL_REG, TFMINI_SIG_COUNT_VAL_MAX); |
||||
|
||||
if (ret != PX4_OK) { |
||||
return ret; |
||||
} |
||||
|
||||
// wait for register write to complete
|
||||
px4_usleep(1_ms); |
||||
|
||||
return OK; |
||||
} |
||||
|
||||
void |
||||
TFmini_i2c::print_registers() |
||||
{ |
||||
_pause_measurements = true; |
||||
PX4_INFO("registers"); |
||||
// wait for a while to ensure the lidar is in a ready state
|
||||
px4_usleep(50_ms); |
||||
|
||||
for (uint8_t reg = 0; reg <= 0x67; reg++) { |
||||
uint8_t val = 0; |
||||
int ret = lidar_transfer(®, 1, &val, 1); |
||||
|
||||
if (ret != OK) { |
||||
printf("%02" PRIx8 ":XX ", reg); |
||||
|
||||
} else { |
||||
printf("%02" PRIx8 ":%02" PRIu8, reg, val); |
||||
} |
||||
|
||||
if (reg % 16 == 15) { |
||||
printf("\n"); |
||||
} |
||||
} |
||||
|
||||
printf("\n"); |
||||
_pause_measurements = false; |
||||
} |
||||
|
||||
|
||||
void |
||||
TFmini_i2c::process_raw_measure(uint16_t distance_raw, uint16_t strength_raw,uint16_t &output_distance_cm) |
||||
{ |
||||
uint16_t strength = (strength_raw); |
||||
const uint16_t MAX_DIST_CM = 1200; |
||||
const uint16_t MIN_DIST_CM = 10; |
||||
|
||||
output_distance_cm = (distance_raw); |
||||
|
||||
if (strength < 100 || strength == 0xFFFF || output_distance_cm > MAX_DIST_CM) { |
||||
/*
|
||||
* From manual: "when the signal strength is lower than 100 or equal to |
||||
* 65535, the detection is unreliable, TFmini Plus will set distance |
||||
* value to 0." - force it to the max distance so status is set to OutOfRangeHigh |
||||
* rather than NoData. |
||||
*/ |
||||
output_distance_cm = MAX_DIST_CM + 100; |
||||
} |
||||
} |
||||
|
||||
|
||||
int |
||||
TFmini_i2c::collect() |
||||
{ |
||||
|
||||
perf_begin(_sample_perf); |
||||
|
||||
uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 }; |
||||
union { |
||||
struct PACKED { |
||||
uint8_t header1; |
||||
uint8_t header2; |
||||
uint16_t distance; |
||||
uint16_t strength; |
||||
uint32_t timestamp; |
||||
uint8_t checksum; |
||||
} val; |
||||
uint8_t arr[11]; |
||||
} u; |
||||
bool ret; |
||||
uint16_t distance; |
||||
|
||||
ret = lidar_transfer(CMD_READ_MEASUREMENT, sizeof(CMD_READ_MEASUREMENT), nullptr, 0); |
||||
if (!ret || !lidar_transfer(nullptr, 0, (uint8_t *)&u, sizeof(u))) { |
||||
|
||||
PX4_DEBUG("error reading from sensor: %d", ret); |
||||
perf_count(_comms_errors); |
||||
|
||||
if (perf_event_count(_comms_errors) % 10 == 0) { |
||||
perf_count(_sensor_resets); |
||||
reset_sensor(); |
||||
} |
||||
perf_end(_sample_perf); |
||||
|
||||
return -EAGAIN; |
||||
} |
||||
|
||||
if (u.val.header1 != 0x59 || u.val.header2 != 0x59 || !check_checksum(u.arr, sizeof(u))) |
||||
return -EAGAIN; |
||||
|
||||
process_raw_measure(u.val.distance, u.val.strength, distance); |
||||
|
||||
accum.sum += distance; |
||||
accum.count++; |
||||
|
||||
const float distance_m = float(distance) * 1e-2f; |
||||
|
||||
if (distance_m == 0) { |
||||
_zero_counter++; |
||||
|
||||
if (_zero_counter == 20) { |
||||
/* we have had 20 zeros in a row - reset the
|
||||
sensor. This is a known bad state of the |
||||
sensor where it returns 16 bits of zero for |
||||
the distance with a trailing NACK, and |
||||
keeps doing that even when the target comes |
||||
into a valid range. |
||||
*/ |
||||
_zero_counter = 0; |
||||
perf_end(_sample_perf); |
||||
perf_count(_sensor_zero_resets); |
||||
return reset_sensor(); |
||||
} |
||||
|
||||
} else { |
||||
_zero_counter = 0; |
||||
} |
||||
// this should be fairly close to the end of the measurement, so the best approximation of the time
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time(); |
||||
|
||||
_px4_rangefinder.update(timestamp_sample, distance_m); |
||||
|
||||
perf_end(_sample_perf); |
||||
return OK; |
||||
|
||||
#if 0 |
||||
|
||||
// read from the sensor
|
||||
uint8_t val[2] {}; |
||||
|
||||
perf_begin(_sample_perf); |
||||
|
||||
// read the high and low byte distance registers
|
||||
uint8_t distance_reg = TFMINI_DISTHIGH_REG | TFMINI_AUTO_INCREMENT; |
||||
int ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val)); |
||||
|
||||
// if the transfer failed or if the high bit of distance is
|
||||
// set then the distance is invalid
|
||||
if (ret < 0 || (val[0] & 0x80)) { |
||||
if (hrt_absolute_time() - _acquire_time_usec > TFMINI_CONVERSION_TIMEOUT) { |
||||
/*
|
||||
NACKs from the sensor are expected when we |
||||
read before it is ready, so only consider it |
||||
an error if more than 100ms has elapsed. |
||||
*/ |
||||
PX4_DEBUG("error reading from sensor: %d", ret); |
||||
perf_count(_comms_errors); |
||||
|
||||
if (perf_event_count(_comms_errors) % 10 == 0) { |
||||
perf_count(_sensor_resets); |
||||
reset_sensor(); |
||||
} |
||||
} |
||||
|
||||
perf_end(_sample_perf); |
||||
// if we are getting lots of I2C transfer errors try
|
||||
// resetting the sensor
|
||||
return ret; |
||||
} |
||||
|
||||
uint16_t distance_cm = (val[0] << 8) | val[1]; |
||||
const float distance_m = float(distance_cm) * 1e-2f; |
||||
|
||||
if (distance_cm == 0) { |
||||
_zero_counter++; |
||||
|
||||
if (_zero_counter == 20) { |
||||
/* we have had 20 zeros in a row - reset the
|
||||
sensor. This is a known bad state of the |
||||
sensor where it returns 16 bits of zero for |
||||
the distance with a trailing NACK, and |
||||
keeps doing that even when the target comes |
||||
into a valid range. |
||||
*/ |
||||
_zero_counter = 0; |
||||
perf_end(_sample_perf); |
||||
perf_count(_sensor_zero_resets); |
||||
return reset_sensor(); |
||||
} |
||||
|
||||
} else { |
||||
_zero_counter = 0; |
||||
} |
||||
|
||||
// this should be fairly close to the end of the measurement, so the best approximation of the time
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time(); |
||||
|
||||
// Relative signal strength measurement, i.e. the strength of
|
||||
// the main signal peak compared to the general noise level.
|
||||
uint8_t signal_strength_reg = TFMINI_SIGNAL_STRENGTH_REG; |
||||
ret = lidar_transfer(&signal_strength_reg, 1, &val[0], 1); |
||||
|
||||
// check if the transfer failed
|
||||
if (ret < 0) { |
||||
if (hrt_elapsed_time(&_acquire_time_usec) > TFMINI_CONVERSION_TIMEOUT) { |
||||
/*
|
||||
NACKs from the sensor are expected when we |
||||
read before it is ready, so only consider it |
||||
an error if more than 100ms has elapsed. |
||||
*/ |
||||
PX4_INFO("signal strength read failed"); |
||||
|
||||
DEVICE_DEBUG("error reading signal strength from sensor: %d", ret); |
||||
perf_count(_comms_errors); |
||||
|
||||
if (perf_event_count(_comms_errors) % 10 == 0) { |
||||
perf_count(_sensor_resets); |
||||
reset_sensor(); |
||||
} |
||||
} |
||||
|
||||
perf_end(_sample_perf); |
||||
// if we are getting lots of I2C transfer errors try
|
||||
// resetting the sensor
|
||||
return ret; |
||||
} |
||||
|
||||
uint8_t TFMINI_signal_strength = val[0]; |
||||
uint8_t signal_quality; |
||||
|
||||
// We detect if V3HP is being used
|
||||
if (_is_v3hp) { |
||||
//Normalize signal strength to 0...100 percent using the absolute signal strength.
|
||||
signal_quality = 100 * math::max(TFMINI_signal_strength - TFMINI_SIGNAL_STRENGTH_MIN_V3HP, 0) / |
||||
(TFMINI_SIGNAL_STRENGTH_MAX_V3HP - TFMINI_SIGNAL_STRENGTH_MIN_V3HP); |
||||
|
||||
} else { |
||||
// Absolute peak strength measurement, i.e. absolute strength of main signal peak.
|
||||
uint8_t peak_strength_reg = TFMINI_PEAK_STRENGTH_REG; |
||||
ret = lidar_transfer(&peak_strength_reg, 1, &val[0], 1); |
||||
|
||||
// check if the transfer failed
|
||||
if (ret < 0) { |
||||
if (hrt_elapsed_time(&_acquire_time_usec) > TFMINI_CONVERSION_TIMEOUT) { |
||||
/*
|
||||
NACKs from the sensor are expected when we |
||||
read before it is ready, so only consider it |
||||
an error if more than 100ms has elapsed. |
||||
*/ |
||||
PX4_INFO("peak strength read failed"); |
||||
|
||||
DEVICE_DEBUG("error reading peak strength from sensor: %d", ret); |
||||
perf_count(_comms_errors); |
||||
|
||||
if (perf_event_count(_comms_errors) % 10 == 0) { |
||||
perf_count(_sensor_resets); |
||||
reset_sensor(); |
||||
} |
||||
} |
||||
|
||||
perf_end(_sample_perf); |
||||
// if we are getting lots of I2C transfer errors try
|
||||
// resetting the sensor
|
||||
return ret; |
||||
} |
||||
|
||||
uint8_t TFMINI_peak_strength = val[0]; |
||||
|
||||
// For v2 and v3 use TFMINI_signal_strength (a relative measure, i.e. peak strength to noise!) to reject potentially ambiguous measurements
|
||||
if (TFMINI_signal_strength <= TFMINI_SIGNAL_STRENGTH_LOW || distance_m < TFMINI_MIN_DISTANCE) { |
||||
signal_quality = 0; |
||||
|
||||
} else { |
||||
//Normalize signal strength to 0...100 percent using the absolute signal peak strength.
|
||||
signal_quality = 100 * math::max(TFMINI_peak_strength - TFMINI_PEAK_STRENGTH_LOW, 0) / |
||||
(TFMINI_PEAK_STRENGTH_HIGH - TFMINI_PEAK_STRENGTH_LOW); |
||||
|
||||
} |
||||
} |
||||
|
||||
_px4_rangefinder.update(timestamp_sample, distance_m, signal_quality); |
||||
|
||||
perf_end(_sample_perf); |
||||
return OK; |
||||
#endif |
||||
} |
||||
|
||||
void TFmini_i2c::start() |
||||
{ |
||||
// reset the report ring and state machine
|
||||
_collect_phase = false; |
||||
|
||||
// schedule a cycle to start things
|
||||
ScheduleNow(); |
||||
} |
||||
|
||||
void TFmini_i2c::RunImpl() |
||||
{ |
||||
// perform collection
|
||||
if (collect() == -EAGAIN) { |
||||
// reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps
|
||||
ScheduleClear(); |
||||
ScheduleOnInterval(7_ms, 87 * 9); |
||||
return; |
||||
} |
||||
|
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */ |
||||
ScheduleDelayed(TFMINI_CONVERSION_INTERVAL); |
||||
#if 0 |
||||
/* collection phase? */ |
||||
if (_collect_phase) { |
||||
|
||||
/* try a collection */ |
||||
if (OK != collect()) { |
||||
PX4_DEBUG("collection error"); |
||||
|
||||
/* if we've been waiting more than 200ms then
|
||||
send a new acquire */ |
||||
if (hrt_elapsed_time(&_acquire_time_usec) > (TFMINI_CONVERSION_TIMEOUT * 2)) { |
||||
_collect_phase = false; |
||||
} |
||||
|
||||
} else { |
||||
/* next phase is measurement */ |
||||
_collect_phase = false; |
||||
|
||||
/*
|
||||
* Is there a collect->measure gap? |
||||
*/ |
||||
if (get_measure_interval() > TFMINI_CONVERSION_INTERVAL) { |
||||
|
||||
/* schedule a fresh cycle call when we are ready to measure again */ |
||||
ScheduleDelayed(get_measure_interval() - TFMINI_CONVERSION_INTERVAL); |
||||
|
||||
return; |
||||
} |
||||
} |
||||
} |
||||
|
||||
if (_collect_phase == false) { |
||||
/* measurement phase */ |
||||
if (OK != measure()) { |
||||
PX4_DEBUG("measure error"); |
||||
|
||||
} else { |
||||
/* next phase is collection. Don't switch to
|
||||
collection phase until we have a successful |
||||
acquire request I2C transfer */ |
||||
_collect_phase = true; |
||||
} |
||||
} |
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */ |
||||
ScheduleDelayed(TFMINI_CONVERSION_INTERVAL); |
||||
#endif |
||||
} |
||||
@ -0,0 +1,203 @@
@@ -0,0 +1,203 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file TFmini_i2c.h |
||||
* @author Allyson Kreft |
||||
* |
||||
* Driver for the PulsedLight Lidar-Lite range finders connected via I2C. |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <drivers/device/i2c.h> |
||||
#include <drivers/drv_hrt.h> |
||||
#include <mathlib/mathlib.h> |
||||
#include <px4_platform_common/defines.h> |
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> |
||||
#include <px4_platform_common/i2c_spi_buses.h> |
||||
#include <drivers/device/device.h> |
||||
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp> |
||||
#include <perf/perf_counter.h> |
||||
|
||||
using namespace time_literals; |
||||
|
||||
/* Configuration Constants */ |
||||
static constexpr uint8_t TFMINI_BASEADDR = 0x10; /* 7-bit address */ |
||||
static constexpr uint8_t TFMINI_BASEADDR_OLD = 0x10; /* 7-bit address */ |
||||
|
||||
static constexpr uint8_t TFMINI_SIG_COUNT_VAL_DEFAULT = 0x80; /* Default maximum acquisition count */ |
||||
|
||||
/* TFMINI Registers addresses */ |
||||
static constexpr uint8_t TFMINI_MEASURE_REG = 0x00; /* Measure range register */ |
||||
static constexpr uint8_t TFMINI_MSRREG_RESET = 0x00; /* reset to power on defaults */ |
||||
static constexpr uint8_t TFMINI_MSRREG_ACQUIRE = 0x04; /* Value to acquire a measurement, version specific */ |
||||
static constexpr uint8_t TFMINI_DISTHIGH_REG = 0x0F; /* High byte of distance register, auto increment */ |
||||
static constexpr uint8_t TFMINI_AUTO_INCREMENT = 0x80; |
||||
static constexpr uint8_t TFMINI_HW_VERSION = 0x41; |
||||
static constexpr uint8_t TFMINI_SW_VERSION = 0x4f; |
||||
static constexpr uint8_t TFMINI_SIGNAL_STRENGTH_REG = 0x0e; |
||||
static constexpr uint8_t TFMINI_PEAK_STRENGTH_REG = 0x0c; |
||||
static constexpr uint8_t TFMINI_UNIT_ID_HIGH = 0x16; |
||||
static constexpr uint8_t TFMINI_UNIT_ID_LOW = 0x17; |
||||
|
||||
static constexpr uint8_t TFMINI_SIG_COUNT_VAL_REG = 0x02; /* Maximum acquisition count register */ |
||||
static constexpr uint8_t TFMINI_SIG_COUNT_VAL_MAX = 0xFF; /* Maximum acquisition count max value */ |
||||
|
||||
static constexpr int TFMINI_SIGNAL_STRENGTH_MIN_V3HP = 70; /* Min signal strength for V3HP */ |
||||
static constexpr int TFMINI_SIGNAL_STRENGTH_MAX_V3HP = 255; /* Max signal strength for V3HP */ |
||||
|
||||
static constexpr int TFMINI_SIGNAL_STRENGTH_LOW = 24; /* Minimum signal strength for a valid measurement */ |
||||
static constexpr int TFMINI_PEAK_STRENGTH_LOW = 135; /* Minimum peak strength for accepting a measurement */ |
||||
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_MAX_DISTANCE{12.00f}; |
||||
static constexpr float TFMINI_MAX_DISTANCE_V2{35.00f}; |
||||
|
||||
// Normal conversion wait time.
|
||||
static constexpr uint32_t TFMINI_CONVERSION_INTERVAL{50_ms}; |
||||
|
||||
// Maximum time to wait for a conversion to complete.
|
||||
static constexpr uint32_t TFMINI_CONVERSION_TIMEOUT{100_ms}; |
||||
|
||||
|
||||
class TFmini_i2c : public device::I2C, public I2CSPIDriver<TFmini_i2c> |
||||
{ |
||||
public: |
||||
TFmini_i2c(I2CSPIBusOption bus_option, const int bus, const uint8_t orientation, int bus_frequency, |
||||
const int address = TFMINI_BASEADDR); |
||||
virtual ~TFmini_i2c(); |
||||
|
||||
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, |
||||
int runtime_instance); |
||||
static void print_usage(); |
||||
|
||||
|
||||
int init(); |
||||
|
||||
/**
|
||||
* Print sensor registers to console for debugging. |
||||
*/ |
||||
void print_registers(); |
||||
|
||||
/**
|
||||
* Initialise the automatic measurement state machine and start it. |
||||
* |
||||
* @note This function is called at open and error time. It might make sense |
||||
* to make it more aggressive about resetting the bus in case of errors. |
||||
*/ |
||||
void start(); |
||||
|
||||
/**
|
||||
* Perform a poll cycle; collect from the previous measurement |
||||
* and start a new one. |
||||
*/ |
||||
void RunImpl(); |
||||
|
||||
protected: |
||||
void custom_method(const BusCLIArguments &cli) override; |
||||
|
||||
void print_status() override; |
||||
|
||||
uint32_t get_measure_interval() const { return TFMINI_CONVERSION_INTERVAL; }; |
||||
|
||||
int measure(); |
||||
|
||||
/**
|
||||
* Reset the sensor to power on defaults plus additional configurations. |
||||
*/ |
||||
int reset_sensor(); |
||||
|
||||
int probe() override; |
||||
|
||||
int read_reg(const uint8_t reg, uint8_t &val); |
||||
|
||||
int write_reg(const uint8_t reg, const uint8_t &val); |
||||
|
||||
int check_checksum(uint8_t *arr, int pkt_len); |
||||
|
||||
private: |
||||
|
||||
int collect(); |
||||
|
||||
/**
|
||||
* LidarLite specific transfer function. This is needed |
||||
* to avoid a stop transition with SCL high |
||||
* |
||||
* @param send Pointer to bytes to send. |
||||
* @param send_len Number of bytes to send. |
||||
* @param recv Pointer to buffer for bytes received. |
||||
* @param recv_len Number of bytes to receive. |
||||
* @return OK if the transfer was successful, -errno |
||||
* otherwise. |
||||
*/ |
||||
int lidar_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len); |
||||
|
||||
/**
|
||||
* Test whether the device supported by the driver is present at a |
||||
* specific address. |
||||
* |
||||
* @param address The I2C bus address to probe. |
||||
* @return True if the device is present. |
||||
*/ |
||||
int probe_address(const uint8_t address); |
||||
|
||||
bool _collect_phase{false}; |
||||
bool _is_v3hp{false}; |
||||
bool _pause_measurements{false}; |
||||
|
||||
uint8_t _hw_version{0}; |
||||
uint8_t _sw_version{0}; |
||||
|
||||
uint16_t _unit_id{0}; |
||||
uint16_t _zero_counter{0}; |
||||
|
||||
uint64_t _acquire_time_usec{0}; |
||||
|
||||
PX4Rangefinder _px4_rangefinder; |
||||
|
||||
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "TFMINI: comms errors")}; |
||||
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "TFMINI: read")}; |
||||
perf_counter_t _sensor_resets{perf_alloc(PC_COUNT, "TFMINI: resets")}; |
||||
perf_counter_t _sensor_zero_resets{perf_alloc(PC_COUNT, "TFMINI: zero resets")}; |
||||
|
||||
void process_raw_measure(uint16_t distance_raw, uint16_t strength_raw,uint16_t &output_distance_cm); |
||||
|
||||
|
||||
struct { |
||||
uint32_t sum; |
||||
uint32_t count; |
||||
} accum; |
||||
}; |
||||
@ -0,0 +1,142 @@
@@ -0,0 +1,142 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file tfmini_i2c_main.cpp |
||||
* @author Allyson Kreft |
||||
* @author Johan Jansen <jnsn.johan@gmail.com> |
||||
* @author Ban Siesta <bansiesta@gmail.com> |
||||
* @author James Goppert <james.goppert@gmail.com> |
||||
* |
||||
*/ |
||||
|
||||
#include <board_config.h> |
||||
#include <px4_platform_common/getopt.h> |
||||
#include <px4_platform_common/module.h> |
||||
|
||||
#include "tfmini_i2c.h" |
||||
|
||||
void |
||||
TFmini_i2c::print_usage() |
||||
{ |
||||
PRINT_MODULE_DESCRIPTION( |
||||
R"DESCR_STR( |
||||
### Description |
||||
|
||||
I2C bus driver for LidarLite rangefinders. |
||||
|
||||
The sensor/driver must be enabled using the parameter SENS_EN_LL40LS. |
||||
|
||||
Setup/usage information: https://docs.px4.io/master/en/sensor/lidar_lite.html
|
||||
)DESCR_STR"); |
||||
|
||||
PRINT_MODULE_USAGE_NAME("tfmini_i2c_main", "driver"); |
||||
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); |
||||
PRINT_MODULE_USAGE_COMMAND("start"); |
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); |
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - forward facing by default", true); |
||||
PRINT_MODULE_USAGE_COMMAND("regdump"); |
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); |
||||
} |
||||
|
||||
I2CSPIDriverBase *TFmini_i2c::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, |
||||
int runtime_instance) |
||||
{ |
||||
// 类实例，传入BusInstanceIterator总线实例迭代器参数，解析 CLI 参数
|
||||
printf("instantiate: bus:%d,ori:%d,freq:%d \n", iterator.bus(), cli.orientation, cli.bus_frequency); |
||||
TFmini_i2c* instance = new TFmini_i2c(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency); |
||||
if (instance == nullptr) { |
||||
PX4_ERR("alloc failed"); |
||||
return nullptr; |
||||
} |
||||
|
||||
if (instance->init() != PX4_OK) { |
||||
delete instance; |
||||
return nullptr; |
||||
} |
||||
|
||||
instance->start(); |
||||
return instance; |
||||
} |
||||
|
||||
void |
||||
TFmini_i2c::custom_method(const BusCLIArguments &cli) |
||||
{ |
||||
print_registers(); |
||||
} |
||||
|
||||
extern "C" __EXPORT int tfmini_i2c_main(int argc, char *argv[]) |
||||
{ |
||||
int ch; |
||||
using ThisDriver = TFmini_i2c; |
||||
BusCLIArguments cli{true, false}; |
||||
cli.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; |
||||
cli.default_i2c_frequency = 400000; |
||||
|
||||
while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { |
||||
switch (ch) { |
||||
case 'R': |
||||
cli.orientation = (enum Rotation)atoi(cli.optArg()); |
||||
break; |
||||
} |
||||
} |
||||
|
||||
const char *verb = cli.optArg(); |
||||
|
||||
if (!verb) { |
||||
ThisDriver::print_usage(); |
||||
return -1; |
||||
} |
||||
|
||||
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_TFMINI_I2C); |
||||
|
||||
if (!strcmp(verb, "start")) { |
||||
printf("%s,cli: drv:%d,ori:%d,freq:%d \n",__FILE__, DRV_DIST_DEVTYPE_TFMINI_I2C, cli.orientation, cli.default_i2c_frequency); |
||||
return ThisDriver::module_start(cli, iterator); |
||||
} |
||||
|
||||
if (!strcmp(verb, "stop")) { |
||||
return ThisDriver::module_stop(iterator); |
||||
} |
||||
|
||||
if (!strcmp(verb, "status")) { |
||||
return ThisDriver::module_status(iterator); |
||||
} |
||||
|
||||
if (!strcmp(verb, "regdump")) { |
||||
return ThisDriver::module_custom_method(cli, iterator); |
||||
} |
||||
|
||||
ThisDriver::print_usage(); |
||||
return -1; |
||||
} |
||||
@ -1 +1 @@
@@ -1 +1 @@
|
||||
Subproject commit 71fc1b81612fa9b5184d5abb93b69d109e9d0e4b |
||||
Subproject commit b3fed06fe822d08d19ab1d2c2f8daf7b7d21951c |
||||
@ -0,0 +1,8 @@
@@ -0,0 +1,8 @@
|
||||
date -R |
||||
starttime=`date +'%Y-%m-%d %H:%M:%S'` |
||||
make px4_fmu-v3_default upload |
||||
endtime=`date +'%Y-%m-%d %H:%M:%S'` |
||||
date -R |
||||
start_seconds=$(date --date="$starttime" +%s); |
||||
end_seconds=$(date --date="$endtime" +%s); |
||||
echo "本次运行时间： "$((end_seconds-start_seconds))"s" |
||||
Loading…
Reference in new issue