Ekf2 not generating all output uORB messages

rcpx4 · November 1, 2018, 8:06pm

I’m just starting to try to integrate external sensors and feed them to ekf2 under PX4 on custom hardware.
I am publishing the following messages with a new PX4 module based on reading my sensors and can see the messages being published.

sensor_accel 250Hz
sensor_gyro 250Hz
sensor_mag 250Hz
vehicle_gps_position 5 Hz
I have set the following EKF2 related params, and assume all others are at default.
param set EKF2_AID_MASK 1
param set EKF2_GPS_CHECK 21
param set EKF2_GPS_MASK 0
param set EKF2_HGT_MODE 1
I see ekf2 is running but ekf2_timestamps is the only message that’s being published by it.
I was expecting to see several messages such as estimator_status, vehicle_local_position, etc.
I must be missing some configuration or other obvious setup.

Can anyone please advise on what could be missing?
Thanks!

bkueng · November 4, 2018, 6:59pm

Hi

Can you provide the output of uorb top and top?

rcpx4 · November 7, 2018, 1:11pm

I added in a fake sensor_baro message and that got ekf2 publishing the expected messages.

Unfortunately, I don’t have the “uorb top” and “top” outputs captured and saved before I added in the fake sensor_baro message, but I don’t think I saw anything unusual in those outputs, i.e. ekf2 was running and top didn’t indicate any excessive resource usage.

While EKF2_HGT_MODE is set to1, I still am not sure whether ekf2 is using the fake sensor_baro message as well or not in computing height. Any thoughts?

Thanks.

bkueng · November 8, 2018, 11:23am

With EKF2_HGT_MODE set to 1 ekf2 should not depend on the sensor_baro message at all. Can you further investigate that?
sensor_baro is turned into the vehicle_air_data topic in sensors module used by ekf2.

rcpx4 · November 13, 2018, 2:08am

I went back and obtained the uorb top and top outputs under both conditions:

Without publishing sensor_data with EKF2_HGT_MODE=1

nsh> param show EKF2_HGT_MODE
Symbols: x = used, + = saved, * = unsaved
x + EKF2_HGT_MODE [128,222] : 1

TOPIC NAME                    INST #SUB #MSG #LOST #QSIZE
task_stack_info                  0    0    1     0 2
vehicle_gps_position             0    4    4     7 1
sensor_accel                     0    1  251     0 1
sensor_gyro                      0    2  251     0 1
sensor_mag                       0    1  251     0 1
actuator_controls_0              0    5  251   227 1
input_rc                         0    3   87    81 1
sensor_preflight                 0    1  251     0 1
sensor_combined                  0    3  251   164 1
vehicle_magnetometer             0    3  251   164 1
manual_control_setpoint          0    3   88     5 1
rc_channels                      0    0   88     0 1
actuator_controls_3              0    1   88     0 1
actuator_outputs                 0    2  333   330 1
multirotor_motor_limits          0    1  333   107 1
ekf2_timestamps                  0    1  251     0 1
vehicle_rates_setpoint           0    2  251     0 1
rate_ctrl_status                 0    1  251     0 1

 PID COMMAND                   CPU(ms) CPU(%)  USED/STACK PRIO(BASE) STATE FD
   0 Idle Task                  102074 56.497   308/ 4096   0 (  0)  READY  3
   1 hpwork                       7775  4.708  1420/ 4076 249 (249)  READY 18
   2 lpwork                        265  0.094  1124/ 4076  50 ( 50)  w:sig  8
   3 init                         2912  0.000  2428/ 4076 100 (100)  w:sem  3
 292 top                           114  1.224  1532/ 2028 255 (255)  RUN    3
 114 vvvvv                        7537  4.519  1860/ 4076 251 (251)  READY  4
 117 bbbbb                        4390  0.000   980/ 2028 100 (100)  w:sig  3
 153 sensors                     13330  5.555  1660/ 2164 249 (249)  w:sem 12
 155 commander                    2080  1.318  1804/ 3212 140 (140)  w:sig 28
 156 commander_low_prio              6  0.000   772/ 2996  50 ( 50)  w:sem 28
 158 ppppppp                      7034  1.883   916/ 2012 100 (100)  w:sig  4
 174 mavlink_if0                 11012  6.403  2044/ 2556 100 (100)  READY 25
 175 mavlink_rcv_if0              1232  0.659  1908/ 2836 175 (175)  READY 25
 250 ekf2                         9599  5.932  2276/ 6572 250 (250)  w:sem 23
 255 mc_att_control              12591  8.192  1228/ 1660 251 (251)  w:sem 16
 261 mc_pos_control                635  0.376  1324/ 1876 250 (250)  w:sem  9
 264 navigator                      20  0.000  1228/ 1764 105 (105)  w:sem 15
 272 logger                       2657  1.600  1316/ 3524 245 (245)  w:sem 24

Processes: 18 total, 6 running, 12 sleeping, max FDs: 54
CPU usage: 42.47% tasks, 1.04% sched, 56.50% idle
Uptime: 171.123s total, 102.075s idle

With sensor_data being published and EKF2_HGT_MODE=1

nsh> param show EKF2_HGT_MODE 
Symbols: x = used, + = saved, * = unsaved
x + EKF2_HGT_MODE [128,222] : 1

update: 1s, num topics: 77
TOPIC NAME                    INST #SUB #MSG #LOST #QSIZE
task_stack_info                  0    0    1     0 2
vehicle_gps_position             0    4    4     3 1
sensor_accel                     0    1  250     0 1
sensor_baro                      0    1  250     0 1
sensor_gyro                      0    2  250     0 1
sensor_mag                       0    1  250     0 1
actuator_controls_0              0    5  250   229 1
input_rc                         0    3   87    82 1
sensor_preflight                 0    1  250     0 1
sensor_combined                  0    3  250   164 1
vehicle_air_data                 0    4  250   242 1
vehicle_magnetometer             0    3  250   164 1
estimator_status                 0    3  125    38 1
vehicle_local_position           0    6  125   220 1
manual_control_setpoint          0    3   87     0 1
rc_channels                      0    0   87     0 1
actuator_controls_3              0    1   87     0 1
actuator_outputs                 0    2  331   329 1
multirotor_motor_limits          0    1  331   107 1
ekf2_timestamps                  0    1  250     0 1
vehicle_attitude                 0    4  250   430 1
vehicle_rates_setpoint           0    2  250     0 1
sensor_bias                      0    3  125   160 1
rate_ctrl_status                 0    1  250     0 1
ekf2_innovations                 0    1  125     0 1

 PID COMMAND                   CPU(ms) CPU(%)  USED/STACK PRIO(BASE) STATE FD
   0 Idle Task                  638027 44.109   348/ 4096   0 (  0)  READY  3
   1 hpwork                      66423  4.901  1412/ 4076 249 (249)  READY 18
   2 lpwork                       2165  0.094  1124/ 4076  50 ( 50)  w:sig  8
   3 init                         2954  0.000  2428/ 4076 100 (100)  w:sem  3
 294 top                            51  1.225  1508/ 2028 255 (255)  RUN    3
 114 vvvvv                       64904  4.995  1780/ 4076 251 (251)  READY  4
 117 bbbbb                        4469  0.000   980/ 2028 100 (100)  w:sig  3
 153 sensors                     89351  6.126  1660/ 2164 249 (249)  w:sem 13
 155 commander                   18778  1.413  1828/ 3212 140 (140)  READY 28
 156 commander_low_prio             54  0.000   804/ 2996  50 ( 50)  w:sem 28
 158 ppppppp                     29558  1.885   896/ 2012 100 (100)  READY  4
 174 mavlink_if0                 89623  6.409  2044/ 2556 100 (100)  READY 30
 175 mavlink_rcv_if0              9886  0.754  1908/ 2836 175 (175)  w:sem 30
 250 ekf2                       221085 16.399  4908/ 6572 250 (250)  w:sem 23
 255 mc_att_control             109744  7.728  1228/ 1660 251 (251)  w:sem 16
 261 mc_pos_control              14395  1.036  1204/ 1876 250 (250)  w:sem  9
 264 navigator                    2758  0.188  1212/ 1764 105 (105)  w:sem 15
 272 logger                      22030  1.602  1316/ 3524 245 (245)  w:sem 29

Processes: 18 total, 7 running, 11 sleeping, max FDs: 54
CPU usage: 54.76% tasks, 1.13% sched, 44.11% idle
Uptime: 1385.192s total, 638.028s idle

From what I can tell, in ekf2_main.cpp, there is:
const bool updated = _ekf.update();
Most of the ekf2 published messages are only published if updated is true.

Tracking down what _ekf.update() is took me to src/lib/ecl/EKF/ekf.cpp
where EKf::update() is defined.
This function returns false if initialiseFilter() returns false.

Digging into initialiseFilter(), it has the following statement:

    		return false;

Now, hgt_count_fail will be true if _hgt_counter is not large enough.
And from what I can tell (though I didn’t follow this part too well),
_hgt_counter is dependent on baro data being present.

So the above could be a potential cause of what I’m seeing.

bkueng · November 20, 2018, 6:37am

You’re right. I asked Paul Riseborough, the author of ekf2, and the reason for the hard baro requirement is that it’s used as a fallback option (in Ekf::controlHeightSensorTimeouts()), if the primary height source times out.

rcpx4 · November 20, 2018, 2:54pm

Thanks for the confirmation!