mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-07-24 15:27:41 +08:00
feat(drivers): add Sony AS-DT1 distance sensor driver (#27769)
* Add Sony AS-DT1 Distance Sensor and Documentation * Ensuring Rotation Parameter works * Sony ASDT1 Sensor and Documentaiton * Fixed formatting and added additional comments * Forgot to stage 1 formatting issue * fix(drivers): use AS-DT1 parameters consistently remove the Sony-ASDT1 -R command line rotation, as well as some other small fixes. Updated the docs to reflect this as well. * refactor(drivers): clarify Sony AS-DT1 variable names --------- Co-authored-by: Sean Hickey <sean@ascendengineer.com> Co-authored-by: Andrew Brahim <35986980+dirksavage88@users.noreply.github.com>
This commit is contained in:
@@ -18,8 +18,9 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_SF45_SERIAL=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_GRF_SERIAL=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_SF45_SERIAL=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_SONY_ASDT1=y
|
||||
CONFIG_DRIVERS_DSHOT=y
|
||||
CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
@@ -35,8 +36,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
|
||||
CONFIG_COMMON_INS=y
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM350=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_LIS2MDL=y
|
||||
CONFIG_DRIVERS_OSD_MSP_OSD=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA228=y
|
||||
@@ -56,14 +55,15 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_ESC_BATTERY=y
|
||||
CONFIG_MODULES_EVENTS=y
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=y
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
|
||||
CONFIG_MODULES_FW_MODE_MANAGER=y
|
||||
CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
|
||||
CONFIG_MODULES_FW_MODE_MANAGER=y
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=y
|
||||
CONFIG_MODULES_GIMBAL=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
@@ -84,7 +84,6 @@ CONFIG_MODULES_MC_POS_CONTROL=y
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
CONFIG_MODULES_NAVIGATOR=y
|
||||
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
|
||||
CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
||||
@@ -118,3 +117,4 @@ CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
||||
CONFIG_ARCH_CHIP_IMXRT=y
|
||||
|
||||
@@ -0,0 +1,55 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" width="980" height="430" viewBox="0 0 980 430" role="img" aria-labelledby="title desc">
|
||||
<title id="title">Sony AS-DT1 UART wiring to flight controller and external power</title>
|
||||
<desc id="desc">AS-DT1 pin 2 connects to external power positive, pin 1 connects to both external power ground and flight controller serial ground, pin 5 UART TX connects to flight controller RX, and pin 6 UART RX connects to flight controller TX.</desc>
|
||||
<defs>
|
||||
<marker id="arrow" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="8" markerHeight="8" orient="auto-start-reverse">
|
||||
<path d="M 0 0 L 10 5 L 0 10 z" fill="#e5e7eb"/>
|
||||
</marker>
|
||||
<style>
|
||||
.box { fill: #111827; stroke: #e5e7eb; stroke-width: 2; rx: 8; }
|
||||
.title { font: 700 22px sans-serif; fill: #ffffff; }
|
||||
.label { font: 16px sans-serif; fill: #f9fafb; }
|
||||
.small { font: 14px sans-serif; fill: #d1d5db; }
|
||||
.pin { font: 15px monospace; fill: #ffffff; }
|
||||
.wire-power { stroke: #dc2626; stroke-width: 4; fill: none; marker-end: url(#arrow); }
|
||||
.wire-ground { stroke: #d1d5db; stroke-width: 4; fill: none; marker-end: url(#arrow); }
|
||||
.wire-tx { stroke: #60a5fa; stroke-width: 4; fill: none; marker-end: url(#arrow); }
|
||||
.wire-rx { stroke: #34d399; stroke-width: 4; fill: none; marker-end: url(#arrow); }
|
||||
.node { fill: #d1d5db; }
|
||||
</style>
|
||||
</defs>
|
||||
|
||||
<rect x="45" y="75" width="250" height="275" class="box"/>
|
||||
<text x="70" y="115" class="title">Sony AS-DT1</text>
|
||||
<text x="70" y="150" class="small">8-pin JST-GH connector</text>
|
||||
<text x="70" y="195" class="pin">Pin 1 GND</text>
|
||||
<text x="70" y="235" class="pin">Pin 2 VCC 12-24V</text>
|
||||
<text x="70" y="275" class="pin">Pin 5 UART TX</text>
|
||||
<text x="70" y="315" class="pin">Pin 6 UART RX</text>
|
||||
|
||||
<rect x="675" y="70" width="250" height="140" class="box"/>
|
||||
<text x="700" y="110" class="title">External Power</text>
|
||||
<text x="700" y="150" class="pin">+ 12-24V</text>
|
||||
<text x="700" y="185" class="pin">- GND</text>
|
||||
|
||||
<rect x="675" y="245" width="250" height="145" class="box"/>
|
||||
<text x="700" y="290" class="title">Flight Controller</text>
|
||||
<text x="700" y="325" class="pin">RX</text>
|
||||
<text x="700" y="355" class="pin">TX</text>
|
||||
<text x="700" y="385" class="pin">GND</text>
|
||||
|
||||
<path d="M 295 230 C 425 230, 535 140, 675 140" class="wire-power"/>
|
||||
<text x="420" y="190" class="label">VCC</text>
|
||||
|
||||
<path d="M 295 190 H 455" class="wire-ground"/>
|
||||
<circle cx="455" cy="190" r="6" class="node"/>
|
||||
<path d="M 455 190 C 535 190, 585 180, 675 180" class="wire-ground"/>
|
||||
<path d="M 455 190 C 550 255, 600 380, 675 380" class="wire-ground"/>
|
||||
<text x="385" y="175" class="label">common ground</text>
|
||||
|
||||
<path d="M 295 270 C 430 270, 535 320, 675 320" class="wire-tx"/>
|
||||
<text x="425" y="298" class="label">AS-DT1 TX to FC RX</text>
|
||||
|
||||
<path d="M 295 310 C 430 310, 535 350, 675 350" class="wire-rx"/>
|
||||
<text x="430" y="355" class="label">AS-DT1 RX to FC TX</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 2.9 KiB |
@@ -248,6 +248,7 @@
|
||||
- [Lightware Lidars (SF/LW/GRF)](sensor/sfxx_lidar.md)
|
||||
- [Lightware SF45 Rotary Lidar](sensor/sf45_rotating_lidar.md)
|
||||
- [Lightware GRF250/GRF500 Gimbal Lidar](sensor/grf_lidar.md)
|
||||
- [Sony AS-DT1 LiDAR](sensor/sony_asdt1.md)
|
||||
- [TeraRanger](sensor/teraranger.md)
|
||||
- [✘ Lanbao PSK-CM8JL65-CC5](sensor/cm8jl65_ir_distance_sensor.md)
|
||||
- [Avionics Anonymous Laser Altimeter UAVCAN Interface (CAN)](dronecan/avanon_laser_interface.md)
|
||||
|
||||
@@ -245,6 +245,7 @@
|
||||
- [Lightware Lidars (SF/LW/GRF)](/sensor/sfxx_lidar.md)
|
||||
- [Lightware SF45 Rotary Lidar](/sensor/sf45_rotating_lidar.md)
|
||||
- [Lightware GRF250/GRF500 Gimbal Lidar](/sensor/grf_lidar.md)
|
||||
- [Sony AS-DT1 LiDAR](/sensor/sony_asdt1.md)
|
||||
- [TeraRanger](/sensor/teraranger.md)
|
||||
- [✘ Lanbao PSK-CM8JL65-CC5](/sensor/cm8jl65_ir_distance_sensor.md)
|
||||
- [Avionics Anonymous Laser Altimeter UAVCAN Interface (CAN)](/dronecan/avanon_laser_interface.md)
|
||||
|
||||
@@ -40332,6 +40332,62 @@ This parameter defines the rotation of the FMU board relative to the platform.
|
||||
| ------- | -------- | -------- | --------- | ------- | ---- | --------- |
|
||||
| ✓ | -1 | 40 | | 0 | | |
|
||||
|
||||
### SENS_ASDT1_CFG (`INT32`) {#SENS_ASDT1_CFG}
|
||||
|
||||
Serial Configuration for Sony AS-DT1 Rangefinder.
|
||||
|
||||
Configure on which serial port to run Sony AS-DT1 Rangefinder.
|
||||
|
||||
**Values:**
|
||||
|
||||
- `0`: Disabled
|
||||
- `6`: UART 6
|
||||
- `101`: TELEM 1
|
||||
- `102`: TELEM 2
|
||||
- `103`: TELEM 3
|
||||
- `104`: TELEM/SERIAL 4
|
||||
- `201`: GPS 1
|
||||
- `202`: GPS 2
|
||||
- `203`: GPS 3
|
||||
- `300`: Radio Controller
|
||||
- `301`: Wifi Port
|
||||
- `401`: EXT2
|
||||
|
||||
| Reboot | minValue | maxValue | increment | default | unit | Read-Only |
|
||||
| ------- | -------- | -------- | --------- | ------- | ---- | --------- |
|
||||
| ✓ | | | | 0 | | |
|
||||
|
||||
### SENS_ASDT1_MODE (`INT32`) {#SENS_ASDT1_MODE}
|
||||
|
||||
Distance measurement range.
|
||||
|
||||
Sony AS-DT1 distance measurement range mode. The driver uses
|
||||
this mode to configure the sensor and publish matching
|
||||
obstacle_distance metadata.
|
||||
|
||||
**Values:**
|
||||
|
||||
- `0`: 30MSTD
|
||||
- `1`: 30M15F
|
||||
- `2`: 30M30F
|
||||
- `3`: 20M
|
||||
- `4`: 40M
|
||||
|
||||
| Reboot | minValue | maxValue | increment | default | unit | Read-Only |
|
||||
| ------- | -------- | -------- | --------- | ------- | ---- | --------- |
|
||||
| ✓ | 0 | 4 | | 0 | | |
|
||||
|
||||
### SENS_ASDT1_ROT (`FLOAT`) {#SENS_ASDT1_ROT}
|
||||
|
||||
Sensor yaw offset.
|
||||
|
||||
Yaw angle offset of the Sony AS-DT1 sensor relative to the
|
||||
vehicle forward direction. Positive values are clockwise.
|
||||
|
||||
| Reboot | minValue | maxValue | increment | default | unit | Read-Only |
|
||||
| ------- | -------- | -------- | --------- | ------- | ---- | --------- |
|
||||
| ✓ | -360 | 360 | | 0 | deg | |
|
||||
|
||||
### SENS_BOARD_X_OFF (`FLOAT`) {#SENS_BOARD_X_OFF}
|
||||
|
||||
Board rotation X (roll) offset.
|
||||
|
||||
@@ -41,6 +41,15 @@ At time of writing PX4 allows you to use the [Lanbao PSK-CM8JL65-CC5](../sensor/
|
||||
|
||||
PX4 v1.14 (and later) supports the [LightWare LiDAR SF45](../sensor/sf45_rotating_lidar.md) rotating lidar which provides 320 degree sensing.
|
||||
|
||||
### Sony AS-DT1 LiDAR
|
||||
|
||||
PX4 supports the [Sony AS-DT1](../sensor/sony_asdt1.md) multipoint LiDAR as a directly connected UART sensor for collision prevention.
|
||||
The driver publishes measurements to `obstacle_distance` with 5 degree bins, using the configured sensor yaw offset.
|
||||
|
||||
The AS-DT1 covers a forward horizontal field of view of about 35 degrees.
|
||||
Only the covered sectors are populated; other directions remain no-data unless covered by another sensor.
|
||||
Configure the sensor as described in the [Sony AS-DT1](../sensor/sony_asdt1.md) guide, then enable collision prevention with [CP_DIST](#CP_DIST).
|
||||
|
||||
### Other Rangefinders
|
||||
|
||||
Other sensors may be enabled, but this requires modification of driver code to set the sensor orientation and field of view.
|
||||
|
||||
@@ -382,6 +382,52 @@ pga460 <command> [arguments...]
|
||||
help
|
||||
```
|
||||
|
||||
## sony_asdt1
|
||||
|
||||
Source: [drivers/distance_sensor/sony](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/distance_sensor/sony)
|
||||
|
||||
### Description
|
||||
|
||||
Sony AS-DT1 serial driver. The driver probes 921600 and 115200 baud, configures
|
||||
the sensor for binary streaming, and publishes multipoint distance measurements.
|
||||
|
||||
Setup/usage information: https://docs.px4.io/main/en/sensor/sony_asdt1.html
|
||||
|
||||
### Examples
|
||||
|
||||
Attempt to start driver on a specified serial device.
|
||||
|
||||
```
|
||||
sony_asdt1 start -d /dev/ttyS4
|
||||
```
|
||||
|
||||
Print the sensor's saved configuration without starting measurements.
|
||||
|
||||
```
|
||||
sony_asdt1 start -d /dev/ttyS4 -s
|
||||
```
|
||||
|
||||
Stop driver.
|
||||
|
||||
```
|
||||
sony_asdt1 stop
|
||||
```
|
||||
|
||||
### Usage {#sony_asdt1_usage}
|
||||
|
||||
```
|
||||
sony_asdt1 <command> [arguments...]
|
||||
Commands:
|
||||
start Start driver
|
||||
-d <val> Serial device
|
||||
[-s] Send flshow and print response instead of starting
|
||||
measurements
|
||||
|
||||
stop Stop driver
|
||||
|
||||
status Print driver status
|
||||
```
|
||||
|
||||
## srf02
|
||||
|
||||
Source: [drivers/distance_sensor/srf02](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/distance_sensor/srf02)
|
||||
|
||||
@@ -29,6 +29,7 @@ There may also be other DroneCAN rangefinders than those listed here.
|
||||
| [LightWare SF45/B] | ToF (IR laser) | ~50 m | UART | ~ | Rotary lidar (collision prevention) |
|
||||
| [MaxBotix I2CXL-MaxSonar-EZ] | Ultrasonic | | I2C | ~ | |
|
||||
| [RaccoonLab Cyphal & DroneCAN µRANGEFINDER] | ToF (IR) | ~0.1 m – ~8 m | DroneCAN, Cyphal | ~ | |
|
||||
| [Sony AS-DT1] | ToF | up to 40 m | UART | ~ | Multipoint distance sensor |
|
||||
| [TeraRanger Evo 60 m] | ToF (IR) | 0.5 m – 60 m | I2C | ~ | |
|
||||
| [TeraRanger Evo 600Hz] | ToF (IR) | 0.75 m – 8 m | I2C | ~ | High update rate (600 Hz) |
|
||||
| [LightWare SF02] _(disc.)_ | ToF (IR laser) | ~50 m | UART | ~ | Discontinued |
|
||||
@@ -55,6 +56,7 @@ There may also be other DroneCAN rangefinders than those listed here.
|
||||
[LightWare SF10/B]: ../sensor/sfxx_lidar.md
|
||||
[LightWare SF10/C]: ../sensor/sfxx_lidar.md
|
||||
[MaxBotix I2CXL-MaxSonar-EZ]: #maxbotix-i2cxl-maxsonar-ez
|
||||
[Sony AS-DT1]: ../sensor/sony_asdt1.md
|
||||
[TeraRanger Evo 60 m]: ../sensor/teraranger.md
|
||||
[TeraRanger Evo 600Hz]: ../sensor/teraranger.md
|
||||
[TeraRanger One]: ../sensor/teraranger.md
|
||||
@@ -109,6 +111,11 @@ Others may be supported via the [RaccoonLab Cyphal and DroneCAN Rangefinder Adap
|
||||
|
||||
PX4 also supports the [LightWare LiDAR SF45 Rotating Lidar](../sensor/sf45_rotating_lidar.md) for [collision prevention](../computer_vision/collision_prevention.md) applications.
|
||||
|
||||
### Sony AS-DT1
|
||||
|
||||
[Sony AS-DT1](../sensor/sony_asdt1.md) is a multipoint distance sensor that connects to PX4 over a UART/serial port.
|
||||
PX4 configures the sensor baud rate and measurement output from the driver.
|
||||
|
||||
### TeraRanger Rangefinders
|
||||
|
||||
[TeraRanger](../sensor/teraranger.md) provide a number of lightweight distance measurement sensors based on infrared Time-of-Flight (ToF) technology.
|
||||
|
||||
111
docs/en/sensor/sony_asdt1.md
Normal file
111
docs/en/sensor/sony_asdt1.md
Normal file
@@ -0,0 +1,111 @@
|
||||
# Sony AS-DT1 LiDAR
|
||||
|
||||
The _Sony AS-DT1 LiDAR_ is a multipoint distance sensor that connects to PX4 over a UART/serial port.
|
||||
|
||||
PX4 configures the sensor for binary output and publishes the measurements to the [`obstacle_distance`](../msg_docs/ObstacleDistance.md) uORB topic.
|
||||
|
||||
## Supported Modes
|
||||
|
||||
The driver supports the AS-DT1 distance measurement range modes listed below.
|
||||
|
||||
| Mode | Maximum Measurement Distance | Minimum Frame Interval | Distance Measurement Points |
|
||||
| ------ | ---------------------------- | ---------------------- | --------------------------- |
|
||||
| 30MSTD | 30 m | 33.33 ms | 576 |
|
||||
| 30M15F | 30 m | 66.66 ms | 576 |
|
||||
| 30M30F | 30 m | 33.33 ms | 288 |
|
||||
| 20M | 20 m | 33.33 ms | 576 |
|
||||
| 40M | 40 m | 66.66 ms | 576 |
|
||||
|
||||
The mode is selected using [SENS_ASDT1_MODE](../advanced_config/parameter_reference.md#SENS_ASDT1_MODE).
|
||||
|
||||
## Collision Prevention Output
|
||||
|
||||
The driver publishes the AS-DT1 data to the [`obstacle_distance`](../msg_docs/ObstacleDistance.md) uORB topic used by [Collision Prevention](../computer_vision/collision_prevention.md).
|
||||
The message uses 72 bins at 5 degree increments around the vehicle.
|
||||
|
||||
The AS-DT1 horizontal field of view is about 35 degrees.
|
||||
The driver maps the sensor's horizontal band into the matching `obstacle_distance` bins and leaves uncovered directions as no-data.
|
||||
[SENS_ASDT1_ROT](../advanced_config/parameter_reference.md#SENS_ASDT1_ROT) sets the yaw offset of this field of view relative to vehicle forward.
|
||||
|
||||
## Hardware Setup
|
||||
|
||||
The sensor can be connected to any unused _serial port_ (UART), such as `TELEM2`, `TELEM3`, or `GPS2`.
|
||||
|
||||
The AS-DT1 uses the 8-pin JST-GH connector for external power and UART.
|
||||
Connect the sensor TX/RX pins to the flight controller UART and share ground between the sensor power supply and flight controller.
|
||||
|
||||

|
||||
|
||||
| AS-DT1 8-pin Connector | Function | Connect To |
|
||||
| ---------------------- | -------------------- | ---------------------------------- |
|
||||
| Pin 1 | GND | External power ground and flight controller serial GND |
|
||||
| Pin 2 | VCC | External 12 V to 24 V power supply |
|
||||
| Pin 5 | UART TX | Flight controller serial RX |
|
||||
| Pin 6 | UART RX | Flight controller serial TX |
|
||||
|
||||
The ground from AS-DT1 pin 1 must be common to both the external power supply and the flight controller serial port.
|
||||
For example, splice or split the AS-DT1 ground wire so it connects to the power supply negative terminal and to the flight controller UART `GND` pin.
|
||||
|
||||
Do not connect the AS-DT1 UART pins directly to RS-232 or RS-422 interfaces.
|
||||
The AS-DT1 UART RX input is 5 V tolerant, and the sensor should be powered from the external 12 V to 24 V supply described in the Sony hardware documentation.
|
||||
|
||||
## Firmware Setup
|
||||
|
||||
The `sony_asdt1` driver must be included in the PX4 firmware for the target board.
|
||||
If the Sony AS-DT1 parameters are not available, add the following line to the target board's `default.px4board` configuration, or enable the driver using [`boardconfig`](../hardware/porting_guide_config.md#px4-menuconfig-setup):
|
||||
|
||||
```plain
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_SONY_ASDT1=y
|
||||
```
|
||||
|
||||
Then rebuild and flash the firmware.
|
||||
|
||||
## Parameter Setup
|
||||
|
||||
Use the Sony application to set the sensor to **Measurement (UART)** mode before connecting it to PX4.
|
||||
|
||||
The driver uses the following parameters:
|
||||
|
||||
| Parameter | Description |
|
||||
| --------- | ----------- |
|
||||
| [SENS_ASDT1_CFG](../advanced_config/parameter_reference.md#SENS_ASDT1_CFG) | Selects the serial port and enables driver autostart on boot. |
|
||||
| [SENS_ASDT1_MODE](../advanced_config/parameter_reference.md#SENS_ASDT1_MODE) | Selects the AS-DT1 measurement range mode. The driver configures the sensor mode and publishes matching `obstacle_distance` metadata. |
|
||||
| [SENS_ASDT1_ROT](../advanced_config/parameter_reference.md#SENS_ASDT1_ROT) | Sets the sensor yaw offset, in degrees, relative to vehicle forward. This is published as `obstacle_distance.angle_offset`; positive values are clockwise. |
|
||||
|
||||
`SENS_ASDT1_MODE` and `SENS_ASDT1_ROT` may not appear until `SENS_ASDT1_CFG` is enabled and the flight controller has rebooted once.
|
||||
|
||||
Reboot the flight controller, or restart the driver manually, after changing these parameters.
|
||||
|
||||
[Configure the serial port](../peripherals/serial_configuration.md) on which the sensor will run using [SENS_ASDT1_CFG](../advanced_config/parameter_reference.md#SENS_ASDT1_CFG).
|
||||
There is no need to set the baud rate for the port, as this is configured by the driver.
|
||||
|
||||
Set [SENS_ASDT1_MODE](../advanced_config/parameter_reference.md#SENS_ASDT1_MODE) to the required measurement mode and reboot the flight controller.
|
||||
|
||||
If the sensor is mounted with a yaw offset from vehicle forward, set [SENS_ASDT1_ROT](../advanced_config/parameter_reference.md#SENS_ASDT1_ROT) to the offset in degrees.
|
||||
Positive values are clockwise.
|
||||
|
||||
## Testing
|
||||
|
||||
You can check the driver state in the _QGroundControl MAVLink Console_:
|
||||
|
||||
```sh
|
||||
sony_asdt1 status
|
||||
```
|
||||
|
||||
You can observe the published measurements with:
|
||||
|
||||
```sh
|
||||
listener obstacle_distance
|
||||
```
|
||||
|
||||
To print the sensor's saved configuration without starting measurements, run:
|
||||
|
||||
```sh
|
||||
sony_asdt1 start -d /dev/ttyS2 -s
|
||||
```
|
||||
|
||||
Replace `/dev/ttyS2` with the serial device for your board and port.
|
||||
|
||||
## Further Information
|
||||
|
||||
- [Modules Reference: Distance Sensor (Driver): sony_asdt1](../modules/modules_driver_distance_sensor.md#sony-asdt1)
|
||||
45
src/drivers/distance_sensor/sony/CMakeLists.txt
Executable file
45
src/drivers/distance_sensor/sony/CMakeLists.txt
Executable file
@@ -0,0 +1,45 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2022-2023 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__distance_sensor__sony_asdt1
|
||||
MAIN sony_asdt1
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
sony_asdt1.cpp
|
||||
sony_asdt1.hpp
|
||||
sony_asdt1_main.cpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
)
|
||||
5
src/drivers/distance_sensor/sony/Kconfig
Executable file
5
src/drivers/distance_sensor/sony/Kconfig
Executable file
@@ -0,0 +1,5 @@
|
||||
menuconfig DRIVERS_DISTANCE_SENSOR_SONY_ASDT1
|
||||
bool "sony_asdt1"
|
||||
default n
|
||||
---help---
|
||||
Enable support for sony_asdt1
|
||||
41
src/drivers/distance_sensor/sony/module.yaml
Executable file
41
src/drivers/distance_sensor/sony/module.yaml
Executable file
@@ -0,0 +1,41 @@
|
||||
module_name: Sony AS-DT1 Rangefinder
|
||||
serial_config:
|
||||
- command: sony_asdt1 start -d ${SERIAL_DEV}
|
||||
port_config_param:
|
||||
name: SENS_ASDT1_CFG
|
||||
group: Sensors
|
||||
|
||||
parameters:
|
||||
- group: Sensors
|
||||
definitions:
|
||||
SENS_ASDT1_MODE:
|
||||
description:
|
||||
short: Distance measurement range
|
||||
long: |
|
||||
Sony AS-DT1 distance measurement range mode. The driver uses
|
||||
this mode to configure the sensor and publish matching
|
||||
obstacle_distance metadata.
|
||||
type: enum
|
||||
values:
|
||||
0: 30MSTD
|
||||
1: 30M15F
|
||||
2: 30M30F
|
||||
3: 20M
|
||||
4: 40M
|
||||
min: 0
|
||||
max: 4
|
||||
default: 0
|
||||
reboot_required: true
|
||||
SENS_ASDT1_ROT:
|
||||
description:
|
||||
short: Sensor yaw offset
|
||||
long: |
|
||||
Yaw angle offset of the Sony AS-DT1 sensor relative to the
|
||||
vehicle forward direction. Positive values are clockwise.
|
||||
type: float
|
||||
decimal: 1
|
||||
default: 0
|
||||
min: -360
|
||||
max: 360
|
||||
unit: deg
|
||||
reboot_required: true
|
||||
1580
src/drivers/distance_sensor/sony/sony_asdt1.cpp
Normal file
1580
src/drivers/distance_sensor/sony/sony_asdt1.cpp
Normal file
File diff suppressed because it is too large
Load Diff
221
src/drivers/distance_sensor/sony/sony_asdt1.hpp
Normal file
221
src/drivers/distance_sensor/sony/sony_asdt1.hpp
Normal file
@@ -0,0 +1,221 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 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 sony_asdt1.hpp
|
||||
*
|
||||
* Sony AS-DT1 serial rangefinder driver.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/obstacle_distance.h>
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
class AS_DT1 : public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
AS_DT1(const char *device, bool print_config_only = false, float yaw_offset_degrees = 0.0f);
|
||||
~AS_DT1() override;
|
||||
|
||||
int init();
|
||||
void print_info();
|
||||
private:
|
||||
enum class ParserState {
|
||||
FindBegin,
|
||||
ReadPayload,
|
||||
ReadEnd,
|
||||
};
|
||||
|
||||
enum class StartupState {
|
||||
SendFormat,
|
||||
WaitFormatPrompt,
|
||||
SendFsync,
|
||||
Streaming,
|
||||
WaitFirstFrame,
|
||||
SyncDrain,
|
||||
SendPromptSync,
|
||||
WaitPromptSync,
|
||||
DrainAfterSync,
|
||||
SendStop,
|
||||
WaitStopPrompt,
|
||||
SendMode,
|
||||
SendReboot,
|
||||
ModeSyncDrain,
|
||||
ModeSendPromptSync,
|
||||
ModeWaitPromptSync,
|
||||
ModeDrainAfterSync,
|
||||
ModeSendStop,
|
||||
ModeWaitStopPrompt,
|
||||
WaitModePrompt,
|
||||
BaudProbeDrain,
|
||||
BaudProbeSendPromptSync,
|
||||
BaudProbeWaitResponse,
|
||||
SendBaud,
|
||||
WaitBaudPrompt,
|
||||
};
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
void Run() override;
|
||||
|
||||
int open_port();
|
||||
void close_port();
|
||||
int write_command_padded(const char *command);
|
||||
int write_prompt_sync();
|
||||
ssize_t drain_input();
|
||||
ssize_t read_startup_response();
|
||||
ssize_t read_baud_probe_response();
|
||||
void record_read(const uint8_t *buffer, ssize_t bytes_read);
|
||||
void reset_parser();
|
||||
void reset_startup_prompt();
|
||||
void set_startup_deadline(hrt_abstime timeout_us);
|
||||
bool startup_deadline_elapsed() const;
|
||||
const char *startup_state_name() const;
|
||||
int read_and_print_response(const char *label);
|
||||
int read_and_parse_available();
|
||||
bool parse_byte(uint8_t byte);
|
||||
int process_frame(const uint8_t *frame, size_t length);
|
||||
|
||||
static uint32_t decode_20bit_raw(const uint8_t *data, size_t sample_index);
|
||||
static uint16_t z_raw_to_distance_cm(uint32_t z_raw);
|
||||
static int sample_to_layout_index(size_t sample_index);
|
||||
static int sample_to_layout_index(size_t sample_index, int32_t mode);
|
||||
static bool short_frame_mode(int32_t mode);
|
||||
static size_t sample_count_for_mode(int32_t mode);
|
||||
static size_t frame_size_for_mode(int32_t mode);
|
||||
static int min_used_row_for_mode(int32_t mode);
|
||||
static int max_used_row_for_mode(int32_t mode);
|
||||
static bool layout_index_to_row_col(int layout_index, int &row, int &col);
|
||||
static int col_to_obstacle_bin(int col);
|
||||
static uint16_t max_distance_for_mode(int32_t mode);
|
||||
static const char *mode_command_for_param(int32_t mode);
|
||||
|
||||
static constexpr size_t COMMAND_BUFFER_SIZE{32};
|
||||
static constexpr size_t COMMAND_PADDING_BYTES{16};
|
||||
static constexpr size_t READ_BUFFER_SIZE{512};
|
||||
static constexpr size_t LAST_READ_CAPTURE_SIZE{64};
|
||||
static constexpr uint8_t READ_DRAIN_LIMIT{8};
|
||||
static constexpr int FORMAT_SETTLE_INTERVAL{200000};
|
||||
static constexpr int REBOOT_SETTLE_INTERVAL{3000000};
|
||||
static constexpr int PROMPT_SYNC_INTERVAL{100000};
|
||||
static constexpr int PROMPT_SYNC_SETTLE_INTERVAL{500000};
|
||||
static constexpr int COMMAND_RESPONSE_TIMEOUT{1000000};
|
||||
static constexpr int FIRST_FRAME_TIMEOUT{2000000};
|
||||
static constexpr unsigned int ASDT1_DESIRED_BAUD{921600};
|
||||
static constexpr unsigned int ASDT1_FALLBACK_BAUD{115200};
|
||||
static constexpr uint8_t PROMPT_SYNC_ATTEMPT_LIMIT{16};
|
||||
static constexpr uint8_t BAUD_PROBE_ATTEMPT_LIMIT{5};
|
||||
static constexpr uint8_t STARTUP_COMMAND_RETRY_LIMIT{3};
|
||||
static constexpr uint8_t STARTUP_DRAIN_READ_LIMIT{8};
|
||||
static constexpr uint8_t FLSHOW_RETRY_LIMIT{5};
|
||||
static constexpr uint8_t BIN_COUNT = sizeof(obstacle_distance_s::distances) / sizeof(
|
||||
obstacle_distance_s::distances[0]);
|
||||
static constexpr size_t ASDT1_MAX_SAMPLE_COUNT{576};
|
||||
static constexpr size_t ASDT1_SHORT_SAMPLE_COUNT{288};
|
||||
static constexpr size_t ASDT1_BINZ_FRAME_SIZE{1440};
|
||||
static constexpr size_t ASDT1_BINZ_SHORT_FRAME_SIZE{720};
|
||||
static constexpr size_t ASDT1_FRAME_BUFFER_SIZE{ASDT1_BINZ_FRAME_SIZE};
|
||||
static constexpr uint16_t MIN_VALID_DISTANCE_CM{20};
|
||||
static constexpr int ASDT1_COLS{24};
|
||||
static constexpr int MIN_USED_ROW{8};
|
||||
static constexpr int MAX_USED_ROW{15};
|
||||
static constexpr int MIN_USED_SHORT_ROW{4};
|
||||
static constexpr int MAX_USED_SHORT_ROW{7};
|
||||
static constexpr float HORIZONTAL_FOV_DEG{35.0f};
|
||||
static constexpr float LEFT_EDGE_DEG{-HORIZONTAL_FOV_DEG / 2.0f};
|
||||
static constexpr float OBSTACLE_INCREMENT_DEG{5.0f};
|
||||
|
||||
obstacle_distance_s _obstacle_distance{};
|
||||
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance)};
|
||||
|
||||
int _fd{-1};
|
||||
int _read_interval_us{2000};
|
||||
char _device[20] {};
|
||||
bool _print_config_only{false};
|
||||
int32_t _range_mode{0};
|
||||
unsigned int _current_baud{ASDT1_DESIRED_BAUD};
|
||||
char _last_command[COMMAND_BUFFER_SIZE] {};
|
||||
size_t _last_command_len{0};
|
||||
ssize_t _last_write{-1};
|
||||
uint64_t _read_attempts{0};
|
||||
uint64_t _bytes_read_total{0};
|
||||
uint64_t _no_data_reads{0};
|
||||
uint64_t _read_errors{0};
|
||||
hrt_abstime _last_read{0};
|
||||
ssize_t _last_read_size{0};
|
||||
uint8_t _last_read_bytes[LAST_READ_CAPTURE_SIZE] {};
|
||||
size_t _last_read_bytes_len{0};
|
||||
uint8_t _frame_buffer[ASDT1_FRAME_BUFFER_SIZE] {};
|
||||
size_t _frame_buffer_len{0};
|
||||
ParserState _parser_state{ParserState::FindBegin};
|
||||
StartupState _startup_state{StartupState::SyncDrain};
|
||||
bool _startup_prompt_seen{false};
|
||||
size_t _startup_prompt_match_index{0};
|
||||
bool _startup_begin_seen{false};
|
||||
size_t _startup_begin_match_index{0};
|
||||
hrt_abstime _startup_deadline{0};
|
||||
uint8_t _startup_sync_attempts{0};
|
||||
uint8_t _startup_stop_attempts{0};
|
||||
uint8_t _startup_format_attempts{0};
|
||||
uint64_t _startup_fsync_attempts{0};
|
||||
uint64_t _startup_frame_wait_baseline{0};
|
||||
uint64_t _startup_mode_attempts{0};
|
||||
uint64_t _startup_baud_set_attempts{0};
|
||||
uint64_t _startup_baud_probe_attempts{0};
|
||||
uint64_t _startup_reboot_attempts{0};
|
||||
bool _startup_baud_probe_done{false};
|
||||
bool _startup_mode_preamble_done{false};
|
||||
uint64_t _startup_prompt_timeouts{0};
|
||||
uint64_t _startup_discarded_bytes{0};
|
||||
size_t _begin_match_index{0};
|
||||
size_t _end_match_index{0};
|
||||
uint64_t _frames_rx{0};
|
||||
uint64_t _frames_pub{0};
|
||||
uint64_t _parser_resets{0};
|
||||
uint64_t _end_marker_failures{0};
|
||||
uint64_t _below_min_distance_samples{0};
|
||||
size_t _last_frame_processed_len{0};
|
||||
size_t _last_sample_count{0};
|
||||
uint8_t _last_valid_bins{0};
|
||||
uint16_t _last_closest_distance{UINT16_MAX};
|
||||
};
|
||||
178
src/drivers/distance_sensor/sony/sony_asdt1_main.cpp
Normal file
178
src/drivers/distance_sensor/sony/sony_asdt1_main.cpp
Normal file
@@ -0,0 +1,178 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "sony_asdt1.hpp"
|
||||
|
||||
#include <parameters/param.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
#include <cstring>
|
||||
|
||||
namespace sony_as_dt1
|
||||
{
|
||||
|
||||
AS_DT1 *g_dev{nullptr};
|
||||
|
||||
static int start(const char *serial_device_path, bool print_config_only, float yaw_offset_degrees)
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
PX4_WARN("already started");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (serial_device_path == nullptr) {
|
||||
PX4_ERR("no device specified");
|
||||
return -1;
|
||||
}
|
||||
|
||||
g_dev = new AS_DT1(serial_device_path, print_config_only, yaw_offset_degrees);
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (g_dev->init() != PX4_OK) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int stop()
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int status()
|
||||
{
|
||||
if (g_dev == nullptr) {
|
||||
PX4_ERR("driver not running");
|
||||
return -1;
|
||||
}
|
||||
|
||||
g_dev->print_info();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int usage()
|
||||
{
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
|
||||
Sony AS-DT1 serial driver. The driver probes 921600 and 115200 baud, configures
|
||||
the sensor for binary streaming, and publishes multipoint distance measurements.
|
||||
|
||||
### Examples
|
||||
|
||||
$ sony_asdt1 start -d /dev/ttyS4
|
||||
$ sony_asdt1 start -d /dev/ttyS4 -s
|
||||
$ sony_asdt1 status
|
||||
$ sony_asdt1 stop
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("sony_asdt1", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
|
||||
PRINT_MODULE_USAGE_PARAM_FLAG('s', "Send flshow and print response instead of starting measurements", true);
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print driver status");
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} // namespace sony_as_dt1
|
||||
|
||||
extern "C" __EXPORT int sony_asdt1_main(int argc, char *argv[])
|
||||
{
|
||||
const char *device_path = nullptr;
|
||||
bool print_config_only = false;
|
||||
float yaw_offset_degrees = 0.0f;
|
||||
int cli_option = 0;
|
||||
int cli_option_index = 1;
|
||||
const char *cli_option_arg = nullptr;
|
||||
|
||||
while ((cli_option = px4_getopt(argc, argv, "d:s", &cli_option_index, &cli_option_arg)) != EOF) {
|
||||
switch (cli_option) {
|
||||
case 'd':
|
||||
device_path = cli_option_arg;
|
||||
break;
|
||||
|
||||
case 's':
|
||||
print_config_only = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
sony_as_dt1::usage();
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (cli_option_index >= argc) {
|
||||
sony_as_dt1::usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
const char *command = argv[cli_option_index];
|
||||
|
||||
if (!strcmp(command, "start")) {
|
||||
const param_t handle = param_find("SENS_ASDT1_ROT");
|
||||
|
||||
if (handle != PARAM_INVALID) {
|
||||
(void)param_get(handle, &yaw_offset_degrees);
|
||||
}
|
||||
|
||||
return sony_as_dt1::start(device_path, print_config_only, yaw_offset_degrees);
|
||||
|
||||
} else if (!strcmp(command, "stop")) {
|
||||
return sony_as_dt1::stop();
|
||||
|
||||
} else if (!strcmp(command, "status")) {
|
||||
return sony_as_dt1::status();
|
||||
}
|
||||
|
||||
sony_as_dt1::usage();
|
||||
return -1;
|
||||
}
|
||||
Reference in New Issue
Block a user