|
|
|
@ -52,10 +52,10 @@ using namespace time_literals;
@@ -52,10 +52,10 @@ using namespace time_literals;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
TFMINI_I2C::TFMINI_I2C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) : |
|
|
|
|
I2C(DRV_DIST_DEVTYPE_TFMINI_I2C, MODULE_NAME, bus, address, bus_frequency), |
|
|
|
|
TFMINI_I2C::TFMINI_I2C(const I2CSPIDriverConfig &config) : |
|
|
|
|
I2C(config), |
|
|
|
|
ModuleParams(nullptr), |
|
|
|
|
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) |
|
|
|
|
I2CSPIDriver(config) |
|
|
|
|
{ |
|
|
|
|
set_device_type(DRV_DIST_DEVTYPE_TFMINI_I2C); |
|
|
|
|
_measure_interval = TFMINI_I2C_MEASURE_INTERVAL; |
|
|
|
@ -117,7 +117,7 @@ TFMINI_I2C::init()
@@ -117,7 +117,7 @@ TFMINI_I2C::init()
|
|
|
|
|
sensor_param_value[_sensor_count].max_range = getTFi2cParam(index,TF_PARAM_MAXD); |
|
|
|
|
sensor_param_value[_sensor_count].min_range = getTFi2cParam(index,TF_PARAM_MIND); |
|
|
|
|
int set_ret = set_param(); |
|
|
|
|
printf("tf[%i]: address 0x%02x ,rototion %d, max:%d, min:%d, set param:%d\n", _sensor_count, \
|
|
|
|
|
printf("tf[%i]: address 0x%02x ,rototion %d, max:%ld, min:%ld, set param:%d\n", _sensor_count, \
|
|
|
|
|
get_device_address(), \
|
|
|
|
|
sensor_param_value[_sensor_count].rotations, \
|
|
|
|
|
sensor_param_value[_sensor_count].max_range, \
|
|
|
|
@ -359,26 +359,6 @@ TFMINI_I2C::custom_method(const BusCLIArguments &cli)
@@ -359,26 +359,6 @@ TFMINI_I2C::custom_method(const BusCLIArguments &cli)
|
|
|
|
|
set_address(cli.i2c_address); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
I2CSPIDriverBase *TFMINI_I2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, |
|
|
|
|
int runtime_instance) |
|
|
|
|
{ |
|
|
|
|
TFMINI_I2C *instance = new TFMINI_I2C(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address); |
|
|
|
|
|
|
|
|
|
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::print_usage() |
|
|
|
|
{ |
|
|
|
@ -386,7 +366,7 @@ TFMINI_I2C::print_usage()
@@ -386,7 +366,7 @@ TFMINI_I2C::print_usage()
|
|
|
|
|
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); |
|
|
|
|
PRINT_MODULE_USAGE_COMMAND("start"); |
|
|
|
|
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); |
|
|
|
|
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x16); |
|
|
|
|
// PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x16);
|
|
|
|
|
PRINT_MODULE_USAGE_COMMAND("set_address"); |
|
|
|
|
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x16); |
|
|
|
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); |
|
|
|
|