It looks like the data going into the estimator should be good. All the values make sense when I log them. I also added code to the estimator call in the lighthouse driver to see if position data was being enqueued or not:
Code: Select all
// Make sure we feed sane data into the estimator
if (!isfinite(ext_pos.pos[0]) || !isfinite(ext_pos.pos[1]) || !isfinite(ext_pos.pos[2])) {
++badCnt; // added
return;
}
ext_pos.stdDev = 0.01;
estimatorEnqueuePosition(&ext_pos);
++goodCnt; // added
Looking at the boot log, it seems that everything starts up OK and the Kalman estimator is selected:
Code: Select all
SYS: Crazyflie 2.1 is up and running!
SYS: Build 64:24de9899cfdf (2019.02 +64) MODIFIED
SYS: I am 0x203937434848500F00340038 and I have 1024KB of flash!
CFGBLK: v1, verification [OK]
DECK_CORE: 1 deck(s) found
DECK_CORE: Calling INIT on driver bcLighthouse4 for deck 0
IMU: BMI088 Gyro I2C connection [OK].
IMU: BMI088 Accel I2C connection [OK]
IMU: BMP388 I2C connection [OK]
ESTIMATOR: Using Kalman (2) estimator
CONTROLLER: Using PID (1) controller
EEPROM: I2C connection [OK].
STAB: Wait for sensor calibration...
SYS: Free heap: 12768 bytes
LH: Lighthouse bootloader version: 2
STAB: Ready to fly.
LH: Firmware version 1 verified, booting deck!
LH: Synchronized!
Got calibration from 39FA9E7A
phase0: 0.060455
phase1: 0.057373
Got calibration from D959BEFB
phase0: 0.076293
phase1: 0.085937