Firmware Structure (Adding VTOL support) - Clarifications

garret

@Eric-Katzfey I tried examining the logs for default modal AI px4 firmware IRIS configuration and my customized firmware with vtol modules enabled. Strangely, the CPU usage seems identical and above 75% even in the default case.

Log plots here: https://drive.google.com/file/d/1UZfAYMRQ-VfYnyf1u__l9NAUHnM8Q0PJ/view?usp=sharing.

I assume one of the cpus is the apps one and the other is the dsp one. This is still strange, since according to my comment above the total cpu usage of the apps was slightly over 70%.

@garret said in Firmware Structure (Adding VTOL support) - Clarifications:

total CPU usage is only slightly over 70%

As for the gazebo sending messages, still looking into it. However, I did some quick diagnostic and found that gazebo's publication rate seems sufficient:

IRIS case

root@nikita-ThinkPad-P16s-Gen-2:/usr/workspace/voxl2_hitl_gazebo# gz topic -d 1 -z /gazebo/modalai_world/modalai/mag
Hz:  89.91
Hz:  88.87
Hz:  76.84
Hz: 105.65
Hz:  87.52
Hz:  90.77
Hz:  83.98
Hz:  85.79
Hz:  57.25
Hz: 134.63
Hz:  82.31
Hz:  88.04
Hz:  48.27
Hz: 227.83
Hz:  87.30
Hz:  75.15
Hz:  60.36
Hz: 105.97
Hz: 101.91
Hz:  92.13
Hz:  89.28
Hz:  83.36
Hz:  88.95
Hz:  88.49
Hz:  55.99
Hz: 156.46
Hz:  85.59
Hz:  90.70
Hz:  50.35
Hz: 129.66
Hz:  92.53
Hz:  87.62
Hz:  84.50
Hz:  66.56
Hz:  90.34
Hz:  89.11
Hz:  73.71
Hz:  58.14
Hz: 117.21
Hz:  88.86
Hz:  49.12
Hz: 179.37
Hz:  91.40
Hz:  89.39
Hz:  62.59
Hz: 108.00
Hz:  87.86
Hz:  75.88
Hz:  84.17
Hz:  77.79
Hz:  72.78
Hz:  87.25
Hz:  93.21
Hz:  64.89
Hz:  73.10
Hz:  92.97
Hz:  53.24
Hz: 196.72
Hz:  90.09
Hz:  89.55
Hz:  48.60
Hz: 121.67
Hz:  68.07
Hz:  96.66
Hz:  91.59
Hz:  89.99
Hz:  91.62
Hz:  72.11
Hz:  91.43
Hz:  78.47
Hz:  91.06
Hz:  81.71
Hz:  72.23
Hz:  93.85
Hz:  82.83
Hz:  84.47
Hz:  73.80
Hz: 114.91
Hz:  85.34
Hz:  95.42
Hz:  79.51
root@nikita-ThinkPad-P16s-Gen-2:/usr/workspace/voxl2_hitl_gazebo# gz topic -d 1 -z /gazebo/modalai_world/modalai/baro
Hz:  45.30
Hz:  33.52
Hz:  40.19
Hz:  38.64
Hz:  39.93
Hz:  45.78
Hz:  41.65
Hz:  42.63
Hz:  44.85
Hz:  39.09
Hz:  45.65
Hz:  36.76
Hz:  41.51
Hz:  43.36
Hz:  34.07
Hz:  47.10
Hz:  45.87
Hz:  39.17
Hz:  53.24
Hz:  40.17
Hz:  37.47
Hz:  37.41
Hz:  50.90
Hz:  37.27
Hz:  53.60
Hz:  38.04
Hz:  45.86
Hz:  43.55
Hz:  43.98
Hz:  44.47
Hz:  40.48
Hz:  39.65
Hz:  44.65
Hz:  43.66
Hz:  41.32
Hz:  42.98
Hz:  40.23
Hz:  44.23
Hz:  42.25
Hz:  30.15

VTOL case

root@nikita-ThinkPad-P16s-Gen-2:/usr/workspace/voxl2_hitl_gazebo# gz topic -d 1 -z /gazebo/standard_vtol_hitl_world/standard_vtol_hitl/mag
Hz: 261.34
Hz: 268.29
Hz: 165.38
Hz: 301.10
Hz: 219.42
Hz: 318.94
Hz: 258.53
Hz: 260.81
Hz: 259.69
Hz: 261.20
Hz: 230.35
Hz: 246.97
Hz: 105.02
Hz: 27011.70
Hz: 584.93
Hz: 245.19
Hz: 210.23
Hz: 137.50
Hz: 317.01
Hz: 217.37
Hz: 213.23
Hz: 245.10
Hz: 102.70
Hz: 7085.62
Hz: 255.86
Hz: 214.26
Hz: 358.80
Hz: 135.56
Hz: 601.92
Hz: 197.99
Hz: 289.66
Hz: 146.01
Hz: 710.52
Hz: 285.12
Hz: 183.18
Hz: 283.64
Hz: 267.36
Hz: 196.66
Hz: 293.54
Hz: 261.75
Hz: 189.79
Hz: 285.15
Hz: 269.74
Hz: 210.62
Hz: 247.68
Hz: 278.95
Hz:  92.07
Hz:   9.93
Hz: 39858.11
Hz: 249.81
Hz: 2192.14
Hz: 263.26
Hz: 255.81
Hz: 268.84
Hz: 204.21
Hz: 119.26
Hz:   9.63
Hz:   9.72
Hz:   9.71
Hz:   9.96
Hz: 2776.08
Hz: 133.24
Hz: 274.92
Hz: 823.84
Hz: 202.26
Hz: 276.13
Hz: 276.10
Hz: 216.36
Hz: 339.81
Hz: 270.43
Hz: 266.76
Hz: 261.38
Hz: 265.82
Hz: 268.36
Hz: 256.52
Hz: 276.77
Hz:  93.41
Hz: 1257.61
Hz: 263.08
Hz: 263.41
Hz: 146.94
Hz: 270.98
Hz: 243.40
Hz: 283.23
Hz: 150.90
Hz: 1511.17
Hz: 274.21
Hz: 258.02
Hz: 144.53
Hz: 2885.13
Hz: 247.98
Hz: 137.59
Hz: 829.43
Hz: 247.74
Hz: 254.85
Hz: 264.31
Hz: 209.19
Hz: 275.45
Hz: 267.43
Hz: 270.33
Hz: 245.57
Hz: 228.72
Hz: 232.26
Hz: 264.85
Hz:  93.60
Hz: 49265.94
Hz: 499.46
Hz: 233.43
Hz: 273.17
Hz: 234.92
Hz: 233.14
Hz: 258.48
Hz: 278.42
Hz: 225.91
Hz: 280.44
Hz: 135.68
Hz: 276.36
Hz: 134.54
Hz: 282.50
root@nikita-ThinkPad-P16s-Gen-2:/usr/workspace/voxl2_hitl_gazebo# gz topic -d 1 -z /gazebo/standard_vtol_hitl_world/standard_vtol_hitl/baro
Hz: 132.49
Hz: 129.18
Hz: 129.00
Hz: 130.49
Hz: 111.43
Hz: 151.83
Hz:  97.87
Hz: 133.37
Hz: 108.81
Hz: 145.29
Hz:  88.34
Hz: 257.54
Hz: 127.70
Hz: 102.37
Hz: 131.12
Hz: 124.66
Hz: 104.16
Hz: 135.17
Hz: 117.07
Hz: 123.88
Hz: 134.75
Hz: 130.16
Hz: 134.76
Hz:  70.86
Hz: 577.87
Hz: 123.25
Hz: 120.28
Hz: 104.71
Hz: 179.62
Hz: 125.18
Hz: 119.61
Hz: 121.50
Hz: 148.17
Hz: 126.87
Hz: 103.36
Hz: 132.89
Hz: 145.11
Hz: 114.83
Hz: 113.25
Hz: 148.53
Hz: 132.36
Hz: 128.08
Hz: 124.23
Hz:  72.73
Hz: 282.04
Hz: 127.52
Hz: 128.52
Hz: 124.72
Hz: 121.43
Hz: 112.05
Hz: 126.82
Hz: 108.68
Hz: 165.72
Hz: 128.03
Hz: 124.98
Hz: 100.47
Hz:   9.51
Hz: 137.65
Hz:  93.98
Hz:  88.69
Hz: 135.25
Hz: 129.01
Hz: 130.18
Hz:  89.62
Hz: 143.84
Hz: 126.38
Hz: 138.17
Hz: 133.42
Hz: 122.09
Hz: 136.52
Hz: 136.22
Hz: 117.27
Hz:  54.04
Hz: 39575.75
Hz: 160.93
Hz: 127.02
Hz:  87.96
Hz: 260.10
Hz:  93.48
Hz: 110.70
Hz: 144.94
Hz: 129.48
Hz: 132.85
Hz: 119.29
Hz: 123.63
Hz: 143.99
Hz: 103.66
Hz: 136.10
Hz:   9.17

Weirdly there is some frequency jumps with VTOL world. Could this be the cause of the error? If you have an idea where these come from , I would appreciate the hint.

Although, when looking at the logs again, this is the magnetometer plots:https://drive.google.com/file/d/1yxWfiHGunIUJAk8WNnSOku5CxXks31so/view?usp=sharing

Apologies for small dot thickness, but it can be noted that magnetometer coming from the vehicle is at twice the frequency that it is coming from the sensor. Ironically, these rates are also off from the frequencies pasted above. For IRIS default the vehicle's magnetometer is at about 2Hz, and same for VTOL. Sensor frequency for IRIS is about 1Hz and same for VTOL. I wonder how this is possible, since for EKF2 to run smoothly it requires at least 5Hz update rate on sensors. I assume I am misinterpreting the data, so if you spot a mistake in my analysis please let me know.

garret

@Eric-Katzfey An update from me:

I have looked at the cpu usage from voxl-portal and got even more confused. When comparing with my previous posts, the results differ again. To summarize:

after adb shell into voxl2, "top" suggested total linux cpu usage of ~70% when running px4
voxl-inspect-services gave about ~15%
PX4 logs give varying cpu usage from 0% to 88% depending on the core
voxl portal outputs about 10% cpu usage on every core when running px4 hitl sim.

Is there a reason all these sources differ so much?

I also looked further into potential reasons for timeout errors on vtol. Keeping px4 parameters identical, the error shows up only when loading a vtol frame in gazebo docker. I wonder if there is anything special that you can think of about how gazebo docker is treating a vtol vs multicopter? I looked into both .world and .sdf files line by line, but nothing stood out. Wondering if there is something I am missing somewhere else.

Lastly, to follow up on super low rate for "sensor_mag" and "vehicle_magnetometer" from the px4 log.
@garret said in Firmware Structure (Adding VTOL support) - Clarifications:

https://drive.google.com/file/d/1yxWfiHGunIUJAk8WNnSOku5CxXks31so/view?usp=sharing

Identical frequency in both multicopter and vtol case makes me wonder if there is a hardcoded bottleneck somewhere for this specific data - could you point it out to me if that is the case?

Hoping to hear from you and thank you for your cooperation.

Eric Katzfey

@garret You have to be careful looking at CPU utilization on the Linux side. Sometimes the number reflects usage on a single core or the usage of a single process and sometimes it is total usage across all cores. The utilization on the DSP is what usually matters more for PX4 since most of the code runs on the DSP.

Eric Katzfey

@garret Unfortunately, I only have experience with the IRIS model in Gazebo so cannot comment on why it would act differently with the VTOL.

Eric Katzfey

@garret This is the code that deals with the Mavlink messages coming in from Gazebo and the sending of Mavlink messages back to Gazebo: https://github.com/modalai/px4-firmware/blob/voxl-dev/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp

garret

@Eric-Katzfey I am 99% confident I found the issue. When running gazebo for IRIS, the real time factor is 1. When running vtol the real time factor drops to ~0.5 or lower. I just tried running gazebo for iris and manually lower the real time factor. i was able to replicate the issue. I think it is because in the mavlink communication script you shared the update for mag and baro happens only in cases where HIL_SENSOR messege is receiver. If the message is delayed, the sensors timeout.

I will start looking into troubleshooting, but if you know what can be done to speed up gazebo's computation or to ease the load of VTOL model, I would appreciate the hint

garret

@garret I also wonder if there is a reason for running gazebo provided in docker except for iris comparability? Could i theoretically run voxl2 with a newer version of gazebo? It seems like those might be more efficient with modelling physics

garret

@Eric-Katzfey good afternoon, still working on this - was wondering if you know how to enable airspeed sensor for the hitl sim? I think I am getting closer to finishing the vtol setup for hitl sim, but need this piece of the puzzle

Eric Katzfey

@garret As best I can tell the diff_pressure field in the Mavlink HIL_SENSOR message sent from Gazebo would have this info but it is an optional field so not sure if Gazebo is actually populating it. If it is then it will be made available within PX4 via the differential_pressure topic.

garret

@Eric-Katzfey gazebo evidently is not publishing diff_pressure but I am confused as to why. from what I have seen online, usually it just works out of the box for people when they build the world. i even checked all the .sdf models in the gazebo classic repo and all the mavlink plugins for fixed wing and for quads are identical. where would this be configured ?

all I see in gazebo is the airspeed topic like such:

gz topic -d 4 -e /gazebo/standard_vtol_hitl_world/standard_vtol_hitl/airspeed/link/airspeed
time {
  sec: 5204
  nsec: 800000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.6875033675151349e-06
longitude_deg: -8.0985729541323173e-06
altitude: 21.067402929998934
velocity_east: 0.0041351439189235657
velocity_north: 0.014366458095080673
velocity_up: -0.077853461599531493

time {
  sec: 5205
  nsec: 0
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.65648519834101e-06
longitude_deg: -8.0864227239372417e-06
altitude: 21.051106543280184
velocity_east: 0.0091959411425546929
velocity_north: 0.01927891127394361
velocity_up: -0.07690062638957737

time {
  sec: 5205
  nsec: 200000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.6218446606961743e-06
longitude_deg: -8.0702784253073972e-06
altitude: 21.037869208492339
velocity_east: 0.0070925229066759387
velocity_north: 0.018342345861654636
velocity_up: -0.059754401868207442

time {
  sec: 5205
  nsec: 400000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.5894898632271678e-06
longitude_deg: -8.06327351450745e-06
altitude: 21.026238181628287
velocity_east: 0.0012297001505018705
velocity_north: 0.018024321587302515
velocity_up: -0.052280255081160854

time {
  sec: 5205
  nsec: 600000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.5569699380192946e-06
longitude_deg: -8.06390211416581e-06
altitude: 21.017491457983851
velocity_east: -0.001618067383164185
velocity_north: 0.017209061152664136
velocity_up: -0.039400112952817

time {
  sec: 5205
  nsec: 800000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.5292924599121225e-06
longitude_deg: -8.0691216897048133e-06
altitude: 21.008538438007236
velocity_east: -0.0040763711507462008
velocity_north: 0.013864325720556073
velocity_up: -0.0538369387758966

time {
  sec: 5206
  nsec: 0
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.5047486190666175e-06
longitude_deg: -8.0773220885855787e-06
altitude: 20.995235768146813
velocity_east: -0.0045764253450751866
velocity_north: 0.013940962132516522
velocity_up: -0.077954621888091963

time {
  sec: 5206
  nsec: 200000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.4757025411756733e-06
longitude_deg: -8.08466180642776e-06
altitude: 20.978572748601437
velocity_east: -0.0033555745179364043
velocity_north: 0.017347668957779176
velocity_up: -0.073148434894389075

time {
  sec: 5206
  nsec: 400000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.4462882142533986e-06
longitude_deg: -8.0904497539150887e-06
altitude: 20.967332033440471
velocity_east: -0.0037972556503345928
velocity_north: 0.014495664657131601
velocity_up: -0.046656718127848877

time {
  sec: 5206
  nsec: 600000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.4263857227869763e-06
longitude_deg: -8.0997367347435748e-06
altitude: 20.958831951953471
velocity_east: -0.0064765043007881376
velocity_north: 0.0076519004378160452
velocity_up: -0.033514086003998168

time {
  sec: 5206
  nsec: 800000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.4205381481821041e-06
longitude_deg: -8.114123401948635e-06
altitude: 20.954045912250876
velocity_east: -0.0096772272248651318
velocity_north: -0.0012674915998885131
velocity_up: -0.017786372762228858

time {
  sec: 5207
  nsec: 0
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.4309173571265127e-06
longitude_deg: -8.13402138663806e-06
altitude: 20.950159355998039
velocity_east: -0.011961145916546649
velocity_north: -0.0095852711307301761
velocity_up: -0.025342314157376228

time {
  sec: 5207
  nsec: 200000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.4519823130523982e-06
longitude_deg: -8.1555908382890325e-06
altitude: 20.942453072406352
velocity_east: -0.011399544148833641
velocity_north: -0.012656908303145411
velocity_up: -0.051105507815038427

time {
  sec: 5207
  nsec: 400000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.4771499703750814e-06
longitude_deg: -8.17306133168418e-06
altitude: 20.930765697732568
velocity_east: -0.0081463581387440729
velocity_north: -0.01625191237728766
velocity_up: -0.054660789923046446

time {
  sec: 5207
  nsec: 600000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.5137210585576305e-06
longitude_deg: -8.189130765359672e-06
altitude: 20.922167201526463
velocity_east: -0.011325751538111934
velocity_north: -0.023812409686585705
velocity_up: -0.035000133967963913

time {
  sec: 5207
  nsec: 800000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.5623360600389142e-06
longitude_deg: -8.2156165213313913e-06
altitude: 20.914503942243755
velocity_east: -0.017165060266171105
velocity_north: -0.029491893881218659
velocity_up: -0.045721487981602388

time {
  sec: 5208
  nsec: 0
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.6179932252596015e-06
longitude_deg: -8.2454542642759144e-06
altitude: 20.903589533641934
velocity_east: -0.015005264062594741
velocity_north: -0.030936998989148992
velocity_up: -0.063867407905587342

time {
  sec: 5208
  nsec: 200000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.6711078160365384e-06
longitude_deg: -8.2693231056106074e-06
altitude: 20.890134408138692
velocity_east: -0.012109170323674366
velocity_north: -0.027940003604763453
velocity_up: -0.057342687350434116

time {
  sec: 5208
  nsec: 400000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.7202299626737322e-06
longitude_deg: -8.2900878427818057e-06
altitude: 20.882502269931138
velocity_east: -0.010841424041424621
velocity_north: -0.026452180041652139
velocity_up: -0.026357105140536331

time {
  sec: 5208
  nsec: 600000000
}
link_name: "standard_vtol_hitl::airspeed::link"
latitude_deg: -6.7652733709128169e-06
longitude_deg: -8.3080433416888439e-06
altitude: 20.877778694964945
velocity_east: -0.00944769266055363
velocity_north: -0.023288046186138157
velocity_up: -0.014516012694954387

and but when it comes to the received mavlink messages:

this is the closest I have to diff_pressure

Eric Katzfey

@garret Here's the source code for our branch of Gazebo: https://github.com/modalai/PX4-SITL_gazebo-classic/tree/voxl-dev