Crazyflie 2.1 EKF2 with flowboard and position mode

[TheLegendaryJedi]

Like we talked about during dev call,

I'm using EK2 with EKF2_AID_MASK using only optical flow. I can fly it in manual and stabilized with no problem. But right now I want to make use of the flow board. I wanted to takeoff and hold position. I can't change to position mode, it just don't give me any feedback.

I' using these configurations (already the dafault on master):

param set-default SYS_MC_EST_GROUP 2
param set-default SYS_HAS_MAG 0
param set-default EKF2_AID_MASK 2
param set-default EKF2_MAG_TYPE 5

I can see that in QGC the distance sensor is working fine. But right now I don't know what to test or debug next to achieve takeoff and hold.

To Reproduce
Steps to reproduce the behavior:

1. Flash the master branch into Crazyflie 2.1
2. Open QGC
3. Try to change to position mode

Logs

• On boot

beef
0x1000e4a2 0xdeadbeef
0x1000e4a1 0xdeadbeef
System fault Occurred on: 2021-02-09-15:12:55
 Type:Hard Fault in file:armv7-m/arm_hardfault.c at line: 134 running task: wq:I2C3
 FW git-hash: 5a8678a52eb53fd1b6ac2424efef1cf5aceade6b
 Build datetime: Mar 12 2021 20:10:28
 Build url: localhost 
 Processor registers: from 0x1000e47c
 r0:0x20005158 r1:0x1000e5c0  r2:0x00000000  r3:0x3253ac56  r4:0x10008070  r5:0x20005158 r6:0x000000f0 r7:0x1000e610
 r8:0x00000001 r9:0x00000000 r10:0x00000000 r11:0x00000000 r12:0x0000000a  sp:0x1000e550 lr:0x08005670 pc:0x080af492
 xpsr:0x61000000 basepri:0x00000080 control:0x00000004
 exe return:0xffffffe9
 Fault status registers: from NVIC
 cfsr:0x00008200 hfsr:0x40000000  dfsr:0x00000000  mmfsr:0x3253ac5e  bfsr:0x3253ac5e afsr:0x00000000
 IRQ stack: 
  top:    0x20000fa8
  sp:     0x20000f18 Valid
  bottom: 0x20000da8
  size:   0x00000200
  used:   00000200
 User stack: 
  top:    0x1000e6e8
  sp:     0x1000e550 Valid
  bottom: 0x1000ddc4
  size:   0x00000924
Invalid Stack! (Corrupted TCB)  Stack base:  1000ddc4 Stack size:  00000924
[boot] -- 2021-02-09-15:12:55 END Fault Log --
sercon: Registering CDC/ACM serial driver
sercon: Successfully registered the CDC/ACM serial driver
HW arch: BITCRAZE_CRAZYFLIE21
FW git-hash: 5a8678a52eb53fd1b6ac2424efef1cf5aceade6b
FW version: 1.11.0 c0 (17498304)
FW git-branch: master
OS: NuttX
OS version: Release 10.0.0 (167772415)
OS git-hash: 9e077c2f806acd5bda5550bd3231bfc7773e170f
Build datetime: Mar 12 2021 20:10:28
Build uri: localhost
Toolchain: GNU GCC, 9.3.1 20200408 (release)
PX4GUID: 000100000000203936564e4b5013004b0055
MCU: STM32F???, rev. 1
INFO  [param] selected parameter default file /fs/microsd/params
Board architecture defaults: /etc/init.d/rc.board_arch_defaults
Board defaults: /etc/init.d/rc.board_defaults
INFO  [dataman] Unknown restart, data manager file '/fs/microsd/dataman' size is 362560 bytes
WARN  [syslink] Started syslink on /dev/ttyS2

INFO  [mavlink] mode: OSD, data rate: 40000 B/s on /dev/bridge0 @ 57600B
Board sensors: /etc/init.d/rc.board_sensors
bmp388 #0 on I2C bus 3
WARN  [bmi088_i2c] _fifo_empty_interval_us 625
WARN  [bmi088_i2c] _fifo_samples 1
WARN  [bmi088_i2c] _fifo_empty_interval_us 625
WARN  [bmi088_i2c] Probe success, ACC_CHIP_ID: 0x1e
bmi088_i2c #0 on I2C bus 3
bmi088_i2c #1 on I2C bus 3
INFO  [vl53l1x] vl53l1x init success
vl53l1x #0 on I2C bus 1 (external)
Board extras: /etc/init.d/rc.board_mavlink
INFO  [mavlink] mode: Config, data rate: 800000 B/s on /dev/ttyACM0 @ 57600B
WARN  [mavlink] stream ODOMETRY not found
WARN  [mavlink] stream ADSB_VEHICLE not found
WARN  [mavlink] stream GPS2_RAW not found
WARN  [mavlink] stream UTM_GLOBAL_POSITION not found
ERROR [mavlink] configure_streams_to_default() failed
INFO  [init] Mixer: /etc/mixers/quad_x_cw.main.mix on /dev/pwm_output0
ekf2 [304:100]
INFO  [ekf2] 0 - selected distance_sensor:0
ERROR [ekf2] 0 - distance_sensor lost, generation 12 -> 14
INFO  [logger] logger started (mode=all)

• When I manually do local_position_estimator start
INFO [local_position_estimator] fuse gps: 1, flow: 0, vis_pos: 0, landing_target: 0, land: 1, pub_agl_z: 0
  And after that the drone reboots!

Thanks

[jinger26]

I'll work on getting @bresch some hardware

[TheLegendaryJedi]

@bresch I tried to change the EK2_MAG_TYPE to MC custom and I still can't change to position mode

[bresch]

@TheLegendaryJedi Yes, because EKF2 waits for mag data to complete the initialization even if it is not used
Most of it was fixed here: PX4/PX4-ECL#931
But something is apparently still blocking

[TheLegendaryJedi]

Is there a way to debug it? Because it does not give me anything on the console.

[bresch]

You could add some printfs indicators in the code to know where it stops.

[TheLegendaryJedi]

Hi @bresch, I made some prints like you suggested and figured that in control.cpp the _flow_sample_delayed never has good samples.

Basically the quality is always 0, and the dt is always 0 too. Any suggestion on what might be the problem?

INFO  [ecl/EKF] ------------------SAMPLE-------------------`
INFO  [ecl/EKF] is_quality_good: 0`
INFO  [ecl/EKF]   _flow_sample_delayed.quality >= _params.flow_qual_min`
INFO  [ecl/EKF]     _flow_sample_delayed.quality: 0`
INFO  [ecl/EKF]     _params.flow_qual_min: 1`
INFO  [ecl/EKF] is_magnitude_good: 1`
INFO  [ecl/EKF] is_tilt_good: 1`
INFO  [ecl/EKF] is_delta_time_good: 0`
INFO  [ecl/EKF]   _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max`
INFO  [ecl/EKF]     _flow_sample_delayed.dt: 0.000000`
INFO  [ecl/EKF]     delta_time_min: 0.051943`
INFO  [ecl/EKF]     _flow_sample_delayed.dt: 0.000000`
INFO  [ecl/EKF]     delta_time_max: 0.077914`
INFO  [ecl/EKF] is_body_rate_comp_available: 1`
ERROR [ekf2] 0 - distance_sensor lost, generation 385 -> 388

[TheLegendaryJedi]

https://review.px4.io/plot_app?log=7ab4c688-20db-4a0f-a5d0-bf9d7d8b229d

[bresch]

Thanks for the log, I'll have a look. I also received a CrazyFlie 2.1 yesterday so I should be able to reproduce your issue soon.

[TheLegendaryJedi]

Thanks for the log, I'll have a look. I also received a CrazyFlie 2.1 yesterday so I should be able to reproduce your issue soon.

Nice! It's preatty awesome.

Just one update. During dev call Matthias and Dagar told I needed to enable pmw3901 parameter and I did. Right now I've manually started the pmw3901:
pmw3901 start -s -b 1

INFO [SPI_I2C] Running on SPI Bus 1
pmw3901: read: 3077 events, 740455us elapsed, 240.64us avg, min 87us max 16947us 957.049us rms
pmw3901: com err: 0 events

EKF2 still don't have a local or glabal postion

ekf2:0 attitude: 1, local position: 0, global position: 0
ekf2: ECL update: 4572 events, 111484us elapsed, 24.38us avg, min 1us max 255us 15.613us rms
ekf2: ECL full update: 4166 events, 2959999us elapsed, 710.51us avg, min 435us max 1774us 177.017us rms
ekf2: IMU message missed: 0 events

But I still can't change to position mode or hold mode.

We have an altitude estimation (checked it in QGC MAVLink inspector).

Flight log:
https://logs.px4.io/plot_app?log=948a52f3-b417-4a25-a674-49be2c0451d2

Some other things that are bothering me and probably you can help:

• If I don't use the SD card I can't write sensor calibration and parameter changes to FRAM;
• For some reason the logs don't have a correct date (always 01/01/1980);
• We still get a lot of Accel timeouts (we could have quick call on this because I already tried a lot things at driver level to solve it and no luck).

[bresch]

Current status: I'm able to get a meaningful local position estimate when doing handheld tests (in the picture below, I'm moving the drone roughly in a square shape and I finish by a circle).

I'll make a flight test when possible

log: https://logs.px4.io/plot_app?log=78b1b6bd-3527-4ca1-bb57-b81a490e7aff

[TheLegendaryJedi]

Nice! Thanks!

Can you send me your parameter file?
I want to test it too.

[bresch]

@TheLegendaryJedi Basically I had to:

• remove && _is_yaw_fusion_inhibited from https://github.com/PX4/PX4-ECL/blob/a7b8afe420f438554ad90bcba0f1f4872325e75b/EKF/ekf_helper.cpp#L518 fix added in PX4/PX4-ECL@5ac4e00
• start the pmw3901 with the -s flag (as you mentioned before)
• set EKF2_MAG_TYPE to mc custom
• set SENS_FLOW_ROT to 180 degrees

Unfortunately it seems that the LiPo died so I cannot do a flight test

[TheLegendaryJedi]

Thanks a lot! I'll do it. Probably only Tuesday next week.

[TheLegendaryJedi]

@TheLegendaryJedi Basically I had to:

• remove && _is_yaw_fusion_inhibited from https://github.com/PX4/PX4-ECL/blob/a7b8afe420f438554ad90bcba0f1f4872325e75b/EKF/ekf_helper.cpp#L518
• start the pmw3901 with the -s flag (as you mentioned before)
• set EKF2_MAG_TYPE to mc custom
• set SENS_FLOW_ROT to 180 degrees

Unfortunately it seems that the LiPo died so I cannot do a flight test

Hi @bresch, just tested it with 1.12 beta 4 and your recommendations.

The position estimate is very slow. Everything is happening with a delay, for example, when you are in position mode all your joystick commands happen with a delay (roll, pitch, yaw, throttle). If I try to auto takeoff and hold (0.5m) it goes up like 4 meters and won't stop, I have to kill switch it. Could it be that the EKF2 with position estimate is too much for STM32F405?

My flight log:

[bresch]

In your log the drone is in position mode and the throttle stick is at 58% which is just enough to takeoff and then the drone lands again due to the error in velocity estimate. I don't see the auto takeoff.
For the rest, the baro is highly affected and you could switch to rng aid only (HGT_MODE) since this is an indoor application. Also, EKF2_MIN_RNG is set to 10cm, which is a lot for this drone. You can set it to almost 0

[TheLegendaryJedi]

Just tried your recomendations and takeoff still drifts a lot:
https://review.px4.io/plot_app?log=09c156ed-f204-4470-91f6-278a01a7c96b

Takeoff with crash:
https://review.px4.io/plot_app?log=8ad23f59-372f-4d0b-9449-f621e4be6667

Regarding position mode it's the same, a lot of lag from sending the commands and the actual response from the drone:
https://review.px4.io/plot_app?log=13250d4a-4f14-414d-ae43-2c1531e3bf79

Is there a way to check the altitude that the drone is trying to achieve and why is it going up and not stopping when it gets to that position?

Thanks!

[slgrobotics]

@bresch @TheLegendaryJedi - if you still have CrazyFlie 2.1 hardware, I would appreciate testing the BMI088/I2C IMU (not necessarily in flight, just move it by hand). I believe the problems you had were in part due to its malfunction.

That's for PR #23141

[bresch]

@slgrobotics Unfortunately my CrazyFlie doesn't connect to QGC anymore for some reason...

[slgrobotics]

@bresch - well, the BMI088/I2C thing seems marginal anyway, and maybe CrazyFlie shouldn't be officially supported (not my call of course). But my last comment here #23141 about SPI-connected devices may deserve consideration (#4). It is worth handling those FFFF's properly and re-testing SPI devices (see #23111 for full description of the problem).

[slgrobotics]

@bresch @dagar - my last comment above, that's the common bug between I2C and SPI, maybe deserving a second look.