px4のソースをみていたら、ドライバの下にTFminiを見つけた。これを修正すればTF03にも使えそう。
まずはTFminiとTF03のプロトコルの違いを調べる。
UARTの設定はどっちも同じ。
TFminiのデータフレーム
TF03のデータフレーム
データフレームは同じ。つまりTFminiのドライバがそのまま使える、はず。
drivers/distance_sensor/tfmini/tfmini_main.cppに以下の記述を発見。
### Examples Attempt to start driver on a specified serial device. $ tfmini start -d /dev/ttyS1 Stop driver $ tfmini stop )DESCR_STR");
今回はSERIAL4のポートを使いたいので、pixhawk1でのSERIAL4の定義を調べる。
PX4-Autopilot/ROMFS/px4fmu_common/init.d/rcS
# # UART mapping on FMUv2/3/4: # # UART1 /dev/ttyS0 IO debug (except v4, there ttyS0 is the wifi) # USART2 /dev/ttyS1 TELEM1 (flow control) # USART3 /dev/ttyS2 TELEM2 (flow control) # UART4 # UART7 CONSOLE # UART8 SERIAL4
上記を見ると、SERIAL4は/dev/tty*には割り当てられてないようである。
今はTELEM2を使ってないので、TELEM2に接続してみることにする。
TELEM2のピンアウト
SERIAL4のピンアウト
TFminiのパラメータ：SENS_TFMINI_CFG
nsh> param show SENS_TFMINI_CFG Symbols: x = used, + = saved, * = unsaved x + SENS_TFMINI_CFG [746,1227] : 102 784/1541 parameters used.
なんか動き出した
nsh> work_queue start WARN [px4_work_queue] already running
140Hzでtfminiが動いている
sh> work_queue status
Work Queue: 8 threads RATE INTERVAL
|__ 1) wq:rate_ctrl
| \__ 1) vehicle_angular_velocity 399.9 Hz 2500 us
|__ 2) wq:SPI1
| |__ 1) l3gd20 1276.0 Hz 784 us (715 us)
| |__ 2) lsm303d 952.4 Hz 1050 us (1050 us)
| |__ 3) mpu6000 399.9 Hz 2500 us
| \__ 4) ms5611 99.2 Hz 10077 us
|__ 3) wq:I2C2
| \__ 1) rgbled 38.6 Hz 25937 us
|__ 4) wq:nav_and_controllers
| |__ 1) sensors 200.0 Hz 5001 us
| |__ 2) vehicle_acceleration 200.0 Hz 5001 us
| |__ 3) vehicle_air_data 74.4 Hz 13435 us
| \__ 4) vehicle_magnetometer 95.1 Hz 10517 us
|__ 5) wq:INS0
| |__ 1) ekf2 200.0 Hz 5001 us
| |__ 2) vehicle_imu 226.5 Hz 4415 us
| \__ 3) vehicle_imu 200.0 Hz 5001 us
|__ 6) wq:hp_default
| |__ 1) battery_status 100.0 Hz 10000 us
| |__ 2) board_adc 100.0 Hz 10000 us (10000 us)
| |__ 3) rc_update 0.0 Hz 0 us
| \__ 4) tone_alarm 0.0 Hz 0 us
|__ 7) wq:UART6
| \__ 1) tfmini 142.9 Hz 7000 us (7000 us)
\__ 8) wq:lp_default
|__ 1) gyro_calibration 50.0 Hz 20000 us (20000 us)不思議なのは、TFMINIの設定パラメータが自動起動になっていないこと。
nsh> param show SENS_TFMINI_CFG Symbols: x = used, + = saved, * = unsaved x SENS_TFMINI_CFG [551,1231] : 0 583/1545 parameters used.
nsh> param show EKF2_HGT_MODE Symbols: x = used, + = saved, * = unsaved x EKF2_HGT_MODE [207,325] : 0 583/1545 parameters used.
SERIAL4=/dev/ttyS6で通信している
nsh> tfmini status state @ 0x20020dc0 Using port '/dev/ttyS6' tfmini: read: 326320 events, 2680774us elapsed, 8.22us avg, min 6us max 1701us 30.457us rms tfmini: com_err: 0 events
一回センサケーブルを外して指し直したから認識されたのか？
それともArdupilot（Arducopter）のコードをアップロードした際に、不揮発メモリに設定されたArdupilotの何かのパラメータが残っていて、それが良い方に働いたとか？PX4とArdupilotが同じパラメータ名を使っているとすると、逆にバグる可能性もあるが。。
と思っていたら、TFMINIのシリアルポートのパラメータが６になっている事に気づいた。
nsh> listener distance_sensor TOPIC: distance_sensor distance_sensor_s timestamp: 2089114922 (606.616788 seconds ago) device_id: 546767925 (Type: 0x97, SERIAL:6 (0x04)) min_distance: 0.4000 max_distance: 12.0000 current_distance: 0.1800 variance: 0.0000 h_fov: 0.0200 v_fov: 0.0200 q: [0.0000, 0.0000, 0.0000, 0.0000] (Roll: 0.0 deg, Pitch: -0.0 deg, Yaw: 0.0 deg) signal_quality: 0 orientation: 25
パラメータのリファレンスでは、6はUART6に割り当てられている。
104: TELEM/SERIAL 4は104とあるが、自分が使っているボードのSERIAL4は６らしい。
しかしコードを修正してアップロード、再起動したら、tfminiのwork_queueが見えない。。
sh> work_queue status
Work Queue: 7 threads RATE INTERVAL
|__ 1) wq:rate_ctrl
| \__ 1) vehicle_angular_velocity 400.0 Hz 2500 us
|__ 2) wq:SPI1
| |__ 1) l3gd20 1276.2 Hz 784 us (715 us)
| |__ 2) lsm303d 952.4 Hz 1050 us (1050 us)
| |__ 3) mpu6000 399.9 Hz 2500 us
| \__ 4) ms5611 99.3 Hz 10073 us
|__ 3) wq:I2C2
| \__ 1) rgbled 38.6 Hz 25910 us
|__ 4) wq:nav_and_controllers
| |__ 1) sensors 200.0 Hz 5000 us
| |__ 2) vehicle_acceleration 200.0 Hz 5001 us
| |__ 3) vehicle_air_data 74.5 Hz 13427 us
| \__ 4) vehicle_magnetometer 95.2 Hz 10505 us
|__ 5) wq:INS0
| |__ 1) ekf2 200.0 Hz 5001 us
| |__ 2) vehicle_imu 227.2 Hz 4401 us
| \__ 3) vehicle_imu 200.0 Hz 5001 us
|__ 6) wq:hp_default
| |__ 1) battery_status 100.0 Hz 9996 us
| |__ 2) board_adc 99.3 Hz 10072 us (10000 us)
| |__ 3) rc_update 0.0 Hz 0 us
| \__ 4) tone_alarm 0.0 Hz 0 us
\__ 7) wq:lp_default
|__ 1) gyro_calibration なんか今度はtfminiが起動していないぞ。
nsh> tfmini status ERROR [tfmini] driver not running
起動してみる
nsh> tfmini start nsh> work_queue status
あれ、tfminiのwork_queueが起動したけど今度はUART3に割り当てられている。
nsh> work_queue status Work Queue: 8 threads RATE INTERVAL |__ 1) wq:rate_ctrl | \__ 1) vehicle_angular_velocity 399.9 Hz 2500 us |__ 2) wq:SPI1 | |__ 1) l3gd20 1275.9 Hz 784 us (715 us) | |__ 2) lsm303d 952.4 Hz 1050 us (1050 us) | |__ 3) mpu6000 400.0 Hz 2500 us | \__ 4) ms5611 99.2 Hz 10076 us |__ 3) wq:I2C2 | \__ 1) rgbled 38.6 Hz 25925 us |__ 4) wq:nav_and_controllers | |__ 1) sensors 200.0 Hz 5001 us | |__ 2) vehicle_acceleration 200.0 Hz 5000 us | |__ 3) vehicle_air_data 74.5 Hz 13432 us | \__ 4) vehicle_magnetometer 95.1 Hz 10515 us |__ 5) wq:INS0 | |__ 1) ekf2 200.0 Hz 5001 us | |__ 2) vehicle_imu 234.6 Hz 4262 us | \__ 3) vehicle_imu 200.0 Hz 5001 us |__ 6) wq:hp_default | |__ 1) battery_status 100.0 Hz 9997 us | |__ 2) board_adc 99.5 Hz 10049 us (10000 us) | |__ 3) rc_update 0.0 Hz 0 us | \__ 4) tone_alarm 0.0 Hz 0 us |__ 7) wq:UART3 | \__ 1) tfmini
tfminiドライバのStatusを確認
|
