PX4常用sh命令
基于NUTTXnsehll
的设计,PX4还自定义了一些sh命令,可以在调试飞控时起到重要作用。这些命令在PX4开发者手册中,都有提到。但是为了方便后期使用,这里再单独做一下记录:
- top 显示当前各个任务的CPU以及RAM的开销 (直接输入top, 可能没有print出来东西,那么就top once)
Usage: top [arguments...]
once print load only once
example:
PID COMMAND CPU(ms) CPU(%) USED/STACK PRIO(BASE) STATE FD
0 Idle Task 25883 47.300 272/ 512 0 ( 0) READY 3
1 hpwork 0 0.000 344/ 1260 249 (249) w:sig 3
2 lpwork 17 0.000 1024/ 1612 50 ( 50) w:sig 3
3 init 891 0.000 2008/ 2924 100 (100) w:sem 3
4 wq:manager 1 0.000 424/ 1252 255 (255) w:sem 4
415 log_writer_file 0 0.000 392/ 1164 60 ( 60) w:sem 3
16 wq:lp_default 54 0.100 944/ 1700 205 (205) w:sem 4
18 wq:hp_default 591 1.100 1112/ 1900 240 (240) w:sem 4
20 dataman 2 0.000 768/ 1204 90 ( 90) w:sem 4
127 wq:SPI1 8670 16.300 1580/ 2332 253 (253) w:sem 4
137 wq:I2C3 356 0.600 892/ 1468 244 (244) w:sem 4
139 wq:SPI4 166 0.300 892/ 2332 250 (250) w:sem 4
199 wq:nav_and_controllers 5840 11.200 5288/ 7196 241 (241) w:sem 4
200 wq:rate_ctrl 4162 7.900 1168/ 1660 255 (255) w:sem 4
207 commander 652 1.200 1440/ 3212 140 (140) w:sig 6
209 commander_low_prio 1 0.000 736/ 2996 50 ( 50) w:sem 6
217 mavlink_if0 3309 7.600 1968/ 2572 100 (100) READY 6
251 gps 31 0.000 1040/ 1676 205 (205) w:sem 4
291 mavlink_if1 612 1.200 1872/ 2484 100 (100) w:sig 4
292 mavlink_rcv_if1 167 0.300 2816/ 4068 175 (175) w:sem 4
302 wq:UART4 257 0.400 752/ 1396 234 (234) w:sem 4
370 wq:attitude_ctrl 395 0.900 1212/ 1668 242 (242) w:sem 4
376 navigator 39 0.100 960/ 1764 105 (105) w:sem 5
398 mavlink_rcv_if0 263 0.400 2776/ 4068 175 (175) w:sem 6
412 logger 196 0.400 2160/ 3644 230 (230) w:sem 3
417 mavlink_shell 1 0.000 976/ 2020 100 (100) w:sem 3
418 top 0 0.000 1456/ 2020 239 (239) RUN 3
Processes: 27 total, 3 running, 24 sleeping, max FDs: 15
CPU usage: 50.00% tasks, 2.70% sched, 47.30% idle
DMA Memory: 5120 total, 1024 used 1536 peak
Uptime: 54.127s total, 25.883s idle
- ps 查看所有的线程信息
example:
PID PRI POLICY TYPE NPX STATE EVENT SIGMASK STACK USED FILLED COMMAND
0 0 FIFO Kthread N-- Ready 00000000 000000 000000 0.0% Idle Task
1 249 FIFO Kthread --- Waiting Signal 00000000 001260 000344 27.3% hpwork
2 50 FIFO Kthread --- Waiting Signal 00000000 001612 001024 63.5% lpwork
3 100 FIFO Task --- Waiting Semaphore 00000000 002924 002008 68.6% init
4 255 FIFO Task --- Waiting Semaphore 00000000 001252 000424 33.8% wq:manager
199 007196 005288 73.4% wq:nav_and_controllers 0x0x816ad54
200 255 FIFO pthread --- Waiting Semaphore 00000000 001660 001168 70.3% wq:rate_ctrl 0x0x8198d9c
137 244 FIFO pthread --- Waiting Semaphore 00000000 001468 000892 60.7% wq:I2C3 0x0x819c5c4
139 250 FIFO pthread --- Waiting Semaphore 00000000 002332 000892 38.2% wq:SPI4 0x0x819c5f4
398 175 FIFO pthread --- Waiting Semaphore 00000000 004068 002776 68.2% mavlink_rcv_if0 0x0x20018970
207 140 FIFO Task --- Waiting Signal 00000000 003212 001440 44.8% commander start
16 205 FIFO pthread --- Waiting Semaphore 00000000 001700 000944 55.5% wq:lp_default 0x0x8157104
209 50 FIFO pthread --- Waiting Semaphore 00000000 002996 000736 24.5% commander_low_prio 0x0
18 240 FIFO pthread --- Waiting Semaphore 00000000 001900 001112 58.5% wq:hp_default 0x0x815b06c
20 90 FIFO Task --- Waiting Semaphore 00000000 001204 000768 63.7% dataman
217 100 FIFO Task --- Waiting Signal 00000000 002572 001968 76.5% mavlink_if0 mavlink start -d /dev/ttyACM0
412 230 FIFO Task --- Waiting Semaphore 00000000 003644 002160 59.2% logger start -b 64 -t
415 60 FIFO pthread --- Waiting Semaphore 00000000 001164 000392 33.6% log_writer_file 0x0x20037120
417 100 FIFO Task --- Running 00000000 002020 001152 57.0% mavlink_shell
291 100 FIFO Task --- Waiting Signal 00000000 002484 001872 75.3% mavlink_if1 mavlink start -d /dev/ttyS1 -b p:SER_TEL1_BAUD -m p:MAV_0_MODE -r p:MAV_0_RATE -f -s -x
292 175 FIFO pthread --- Waiting Semaphore 00000000 004068 002816 69.2% mavlink_rcv_if1 0x0x2001c3a0
302 234 FIFO pthread --- Waiting Semaphore 00000000 001396 001048 75.0% wq:UART4 0x0x819c62c
370 242 FIFO pthread --- Waiting Semaphore 00000000 001668 001212 72.6% wq:attitude_ctrl 0x0x816908c
376 105 FIFO Task --- Waiting Semaphore 00000000 001764 000960 54.4% navigator start
251 205 FIFO Task --- Waiting Semaphore 00000000 001676 001040 62.0% gps start -d /dev/ttyS0 -b p:SER_GPS1_BAUD
127 253 FIFO pthread --- Waiting Semaphore 00000000 002332 001580 67.7% wq:SPI1 0x0x819c5dc
-
work_queue 查看所有的工作队列。工作队列是各个线程中的子任务(这些线程开头都是wq:) 这里有一个不错的有关task和work_queue的解析 ,下面这个时序图摘自上文原文。
-
值的注意的是ScheduleOnInterval调用的schedule_trampoline,其实也就是在调用ScheduleNow()。这说明,ScheduleNow()只能起到一次作用,它可以在初始化或者通过中断触发来进行一次Run()函数的运行!
void ScheduledWorkItem::schedule_trampoline(void *arg) { ScheduledWorkItem *dev = static_cast<ScheduledWorkItem *>(arg); dev->ScheduleNow(); }
此外,
ScheduleDelayed(us)
这个函数也是很有用处,在传感器初始化配置时可以使用 eg:ICM42688P。hrt_call_after
与hrt_call_every
在命名上,可以很明显地看出其功能的不同。ScheduleDelayed(us)
是过多少us后,再调度这个workitem执行,ScheduleOnInterval(us,delay_us
是每隔多久调度一次(使用时,delay_us可以缺省)。void ScheduledWorkItem::ScheduleDelayed(uint32_t delay_us) { hrt_call_after(&_call, delay_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this); } void ScheduledWorkItem::ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us) { hrt_call_every(&_call, delay_us, interval_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this); }
-
Usage: work_queue <command> [arguments...]
Commands:
start
stop
status print status info
example:
Work Queue: 9 threads RATE INTERVAL
|__ 1) wq:rate_ctrl
| |__ 1) mc_rate_control 800.6 Hz 1249 us
| |__ 2) pwm_out 799.5 Hz 1251 us
| \__ 3) vehicle_angular_velocity 800.6 Hz 1249 us
|__ 2) wq:SPI1
| |__ 1) bmi055 997.3 Hz 1003 us
| |__ 2) bmi055 1043.4 Hz 958 us
| |__ 3) icm20602 800.3 Hz 1250 us
| \__ 4) icm20689 798.3 Hz 1253 us
|__ 3) wq:SPI4
| \__ 1) ms5611 99.5 Hz 10048 us
|__ 4) wq:I2C3
| \__ 1) ist8310 88.9 Hz 11245 us
|__ 5) wq:attitude_ctrl
| \__ 1) mc_att_control 200.2 Hz 4996 us
|__ 6) wq:nav_and_controllers
| |__ 1) ekf2 200.2 Hz 4996 us
| |__ 2) land_detector 92.2 Hz 10846 us
| |__ 3) mc_hover_thrust_estimator 0.0 Hz 0 us
| |__ 4) mc_pos_control 50.0 Hz 19993 us
| |__ 5) sensors 200.1 Hz 4999 us
| |__ 6) vehicle_acceleration 208.5 Hz 4796 us
| |__ 7) vehicle_air_data 74.6 Hz 13410 us
| |__ 8) vehicle_imu 200.1 Hz 4998 us
| |__ 9) vehicle_imu 199.9 Hz 5003 us
| \__10) vehicle_imu 200.2 Hz 4996 us
|__ 7) wq:hp_default
| |__ 1) adc 100.0 Hz 9999 us (10000 us)
| |__ 2) battery_status 100.0 Hz 10000 us (10000 us)
| |__ 3) rc_update 0.0 Hz 0 us
| |__ 4) safety_button 30.3 Hz 32997 us (33000 us)
| \__ 5) tone_alarm 10.1 Hz 99473 us
|__ 8) wq:UART4
| \__ 1) rc_input 250.0 Hz 4000 us (4000 us)
\__ 9) wq:lp_default
|__ 1) load_mon 2.0 Hz 499696 us (500000 us)
|__ 2) rgbled_pwm 39.8 Hz 25130 us
\__ 3) send_event 30.0 Hz 33325 us (33333 us)
- uorb 主要可以用于查看当前的uorb话题【uorb top -1 (是数字1)较为常用】
Usage: uorb <command> [arguments...]
Commands:
start
status Print topic statistics
top Monitor topic publication rates
[-a] print all instead of only currently publishing topics with subscribers
[-1] run only once, then exit
[<filter1> [<filter2>]] topic(s) to match (implies -a)
example:
update: 1s, num topics: 76
TOPIC NAME INST #SUB RATE #Q SIZE
actuator_armed 0 6 2 1 24
actuator_controls_0 0 6 801 1 48
actuator_outputs 0 3 800 1 80
adc_report 0 1 100 1 96
battery_status 0 6 200 1 112
battery_status 1 6 200 1 112
commander_state 0 1 2 1 16
cpuload 0 4 2 1 16
estimator_innovation_test_ratios 0 1 99 1 144
estimator_innovation_variances 0 1 99 1 144
estimator_innovations 0 1 99 1 144
estimator_sensor_bias 0 4 99 1 56
estimator_status 0 4 99 1 288
input_rc 0 4 71 1 72
multirotor_motor_limits 0 2 800 1 16
rate_ctrl_status 0 1 801 1 24
safety 0 2 1 1 16
sensor_accel 0 4 800 8 48
sensor_accel 1 3 799 8 48
sensor_accel 2 3 1044 8 48
sensor_baro 0 2 74 1 32
sensor_combined 0 3 200 1 48
sensor_gyro 0 4 800 8 40
sensor_gyro 1 3 799 8 40
sensor_gyro 2 3 993 8 40
sensor_mag 0 3 89 1 48
sensor_preflight 0 1 200 1 24
system_power 0 2 100 1 24
telemetry_status 0 2 1 1 104
telemetry_status 1 2 1 1 104
vehicle_acceleration 0 1 200 1 32
vehicle_air_data 0 8 19 1 40
vehicle_angular_acceleration 0 1 800 1 32
vehicle_angular_velocity 0 5 800 1 32
vehicle_attitude 0 6 200 1 48
vehicle_control_mode 0 8 2 1 32
vehicle_imu 0 3 200 1 56
vehicle_imu 1 3 200 1 56
vehicle_imu 2 3 200 1 56
vehicle_imu_status 0 4 9 1 56
vehicle_imu_status 1 4 10 1 56
vehicle_imu_status 2 4 9 1 56
vehicle_land_detected 0 9 1 1 24
vehicle_local_position 0 11 99 1 160
vehicle_magnetometer 0 3 89 1 24
vehicle_odometry 0 1 99 1 256
vehicle_status 0 17 2 1 56
vehicle_status_flags 0 1 2 1 40
- sensors status (传感器数据状态监视) sensors也是PX4重要的一个module,它负责处理所有的传感器信息,比如进行低通滤波,多传感器的冗余选择等,将原始的传感器驱动程序获得的数据进行适当地处理。
nsh> sensors status
INFO [sensors] selected gyro: 3670026 (0)
INFO [data_validator] validator: best: 0, prev best: 0, failsafe: NO (0 events)
INFO [data_validator] sensor #0, prio: 100, state: OK
INFO [data_validator] val: -0.0010, lp: -0.0019 mean dev: -0.0000 RMS: 0.0043 conf: 1.0000
INFO [data_validator] val: -0.0014, lp: -0.0010 mean dev: -0.0000 RMS: 0.0020 conf: 1.0000
INFO [data_validator] val: 0.0001, lp: -0.0005 mean dev: 0.0000 RMS: 0.0010 conf: 1.0000
INFO [data_validator] sensor #1, prio: 100, state: OK
INFO [data_validator] val: -0.0044, lp: -0.0031 mean dev: 0.0000 RMS: 0.0042 conf: 1.0000
INFO [data_validator] val: 0.0014, lp: 0.0010 mean dev: -0.0000 RMS: 0.0020 conf: 1.0000
INFO [data_validator] val: -0.0002, lp: -0.0001 mean dev: -0.0000 RMS: 0.0011 conf: 1.0000
INFO [data_validator] sensor #2, prio: 75, state: OK
INFO [data_validator] val: 0.0004, lp: -0.0003 mean dev: 0.0000 RMS: 0.0056 conf: 1.0000
INFO [data_validator] val: -0.0030, lp: -0.0005 mean dev: 0.0000 RMS: 0.0036 conf: 1.0000
INFO [data_validator] val: -0.0028, lp: -0.0007 mean dev: 0.0000 RMS: 0.0031 conf: 1.0000
INFO [sensors] selected accel: 3670026 (0)
INFO [data_validator] validator: best: 0, prev best: 0, failsafe: NO (0 events)
INFO [data_validator] sensor #0, prio: 100, state: OK
INFO [data_validator] val: 0.1830, lp: 0.1770 mean dev: -0.0000 RMS: 0.0114 conf: 1.0000
INFO [data_validator] val: 0.0054, lp: 0.0205 mean dev: -0.0000 RMS: 0.0183 conf: 1.0000
INFO [data_validator] val: -9.6658, lp: -9.6973 mean dev: -0.0001 RMS: 0.0619 conf: 1.0000
INFO [data_validator] sensor #1, prio: 100, state: OK
INFO [data_validator] val: 0.1572, lp: 0.1594 mean dev: -0.0001 RMS: 0.0146 conf: 1.0000
INFO [data_validator] val: -0.0000, lp: 0.0140 mean dev: -0.0001 RMS: 0.0204 conf: 1.0000
INFO [data_validator] val: -9.7818, lp: -9.8008 mean dev: -0.0000 RMS: 0.0608 conf: 1.0000
INFO [data_validator] sensor #2, prio: 75, state: OK
INFO [data_validator] val: -0.0745, lp: -0.0491 mean dev: 0.0000 RMS: 0.0174 conf: 1.0000
INFO [data_validator] val: -0.1883, lp: -0.1693 mean dev: -0.0000 RMS: 0.0197 conf: 1.0000
INFO [data_validator] val: -9.7579, lp: -9.7845 mean dev: -0.0001 RMS: 0.0621 conf: 1.0000
INFO [sensors] selected mag: 396825 (0)
INFO [data_validator] validator: best: 0, prev best: 0, failsafe: NO (0 events)
INFO [data_validator] sensor #0, prio: 75, state: OK
INFO [data_validator] val: 0.0021, lp: 0.0049 mean dev: -0.0000 RMS: 0.0015 conf: 1.0000
INFO [data_validator] val: 0.1681, lp: 0.1683 mean dev: 0.0000 RMS: 0.0012 conf: 1.0000
INFO [data_validator] val: -0.0139, lp: -0.0177 mean dev: -0.0000 RMS: 0.0033 conf: 1.0000
INFO [vehicle_air_data] selected barometer: 3997730 (0)
INFO [data_validator] validator: best: 0, prev best: 0, failsafe: NO (0 events)
INFO [data_validator] sensor #0, prio: 75, state: OK
INFO [data_validator] val: 94513.0000, lp: 94516.2891 mean dev: -0.1046 RMS: 2.6126 conf: 1.0000
INFO [data_validator] val: 27.8700, lp: 27.8496 mean dev: 0.0315 RMS: 0.0150 conf: 1.0000
INFO [data_validator] val: 0.0000, lp: 0.0000 mean dev: 0.0000 RMS: 0.0000 conf: 1.0000
INFO [sensors] Airspeed status:
INFO [data_validator] no data
INFO [vehicle_acceleration] selected sensor: 3670026 (0), rate: 800.6 Hz
INFO [vehicle_acceleration] estimated bias: [0.0000 0.0000 0.0000]
INFO [sensor_calibration] ACC 3670026 EN: 1, offset: [0.0556 -0.0322 0.0141] scale: [0.9992 1.0003 1.0002]
INFO [vehicle_angular_velocity] selected sensor: 3670026 (0), rate: 800.6 Hz
INFO [vehicle_angular_velocity] estimated bias: [0.0000 0.0000 0.0000]
INFO [sensor_calibration] GYRO 3670026 EN: 1, offset: [-0.0317 0.0321 0.0104]
INFO [vehicle_imu] IMU ID: 3670026, accel interval: 1248.9 us, gyro interval: 1248.9 us
vehicle_imu: accel data gap: 1 events
vehicle_imu: gyro data gap: 1 events
vehicle_imu: accel update interval: 181828 events, 1249.04us avg, min 971us max 1481us 23.714us rms
vehicle_imu: gyro update interval: 181832 events, 1249.04us avg, min 971us max 1481us 23.714us rms
INFO [sensor_calibration] ACC 3670026 EN: 1, offset: [0.0556 -0.0322 0.0141] scale: [0.9992 1.0003 1.0002]
INFO [sensor_calibration] GYRO 3670026 EN: 1, offset: [-0.0317 0.0321 0.0104]
INFO [vehicle_imu] IMU ID: 3932170, accel interval: 1249.4 us, gyro interval: 1249.4 us
vehicle_imu: accel data gap: 1 events
vehicle_imu: gyro data gap: 1 events
vehicle_imu: accel update interval: 181586 events, 1250.85us avg, min 830us max 1675us 31.126us rms
vehicle_imu: gyro update interval: 181586 events, 1250.85us avg, min 830us max 1675us 31.126us rms
INFO [sensor_calibration] ACC 3932170 EN: 1, offset: [-0.0909 0.1831 -0.1208] scale: [0.9980 0.9986 0.9935]
INFO [sensor_calibration] GYRO 3932170 EN: 1, offset: [0.0112 -0.0308 0.0091]
INFO [vehicle_imu] Accel ID: 4259850, interval: 955.2 us, Gyro ID: 4325386, interval: 1002.3 us
vehicle_imu: accel data gap: 1 events
vehicle_imu: gyro data gap: 1 events
vehicle_imu: accel update interval: 237163 events, 957.82us avg, min 48us max 2114us 162.976us rms
vehicle_imu: gyro update interval: 226733 events, 1001.88us avg, min 46us max 1395us 157.701us rms
INFO [sensor_calibration] ACC 4259850 EN: 1, offset: [0.1842 0.2454 0.1875] scale: [0.9882 0.9913 0.9830]
INFO [sensor_calibration] GYRO 4325386 EN: 1, offset: [0.0013 0.0015 0.0007]
- dmsg DeBug消息打印
example:
sercon: Registering CDC/ACM serial driver
sercon: Successfully registered the CDC/ACM serial driver
HW arch: PX4_FMU_V5
HW type: V560
HW version: 0x00000006
HW revision: 0x00000000
FW git-hash: 71db0903a910d35a18f10d74c851446a6f66d61f
FW version: Release 1.11.0 (17498367)
OS: NuttX
OS version: Release 8.2.0 (134349055)
OS git-hash: ec20f2e6c5cc35b2b9bbe942dea55eabb81297b6
Build datetime: Mar 12 2022 18:35:12
Build uri: localhost
Toolchain: GNU GCC, 9.3.1 20200408 (release)
PX4GUID: 0002000000003533383731385118002a002c
MCU: STM32F76xxx, rev. Z
INFO [param] selected parameter default file /fs/mtd_params
INFO [tune_control] Publishing standard tune 1
Board defaults: /etc/init.d/rc.board_defaults
INFO [dataman] Unknown restart, data manager file '/fs/microsd/dataman' size is 362560 bytes
Board sensors: /etc/init.d/rc.board_sensors
icm20602 #0 on SPI bus 1 (devid=0x38)
icm20689 #0 on SPI bus 1 (devid=0x3c)
bmi055 #0 on SPI bus 1 (devid=0x41)
bmi055 #1 on SPI bus 1 (devid=0x42)
ist8310 #0 on I2C bus 3
ms5611 #0 on SPI bus 4 (devid=0x3d)
Board extras: /etc/init.d/rc.board_mavlink
INFO [mavlink] mode: Config, data rate: 800000 B/s on /dev/ttyACM0 @ 57600B
Starting Main GPS on /dev/ttyS0
Starting MAVLink on /dev/ttyS1
INFO [mavlink] mode: Normal, data rate: 1200 B/s on /dev/ttyS1 @ 57600B
INFO [init] Mixer: /etc/mixers/quad_x.main.mix on /dev/pwm_output0
Addons script: /fs/microsd/etc/extras.txt
INFO [logger] logger started (mode=all)
NuttShell (NSH)
nsh> INFO [ecl/EKF] reset position to last known position
INFO [ecl/EKF] reset velocity to zero
INFO [ecl/EKF] 9901680: EKF aligned, (baro hgt, IMU buf: 18, OBS buf: 14)
INFO [mavlink] Starting mavlink shell
WARN [commander] Manual control lost
- listener 监听uorb消息,用来内部调试非常好用。可惜也是有bug, 后面的一些选项不一定都能启用
Usage: listener <command> [arguments...]
Commands:
<topic_name> uORB topic name
[-i <val>] Topic instance
default: 0
[-n <val>] Number of messages
default: 1
[-r <val>] Subscription rate (unlimited if 0)
default: 0