Lighthouse working but no values from estimator
Posted: Tue Jun 04, 2019 3:44 am
I have a lighthouse deck reading two V1 lighthouses seemingly successfully: If I log lighthouse x/y/z position the values are highly accurate to within 10mm or so, the angles make sense and change as you would expect them to, but I usually do not see any values at all when I log stateEstimate.x/.y/.z. Sometimes the values are zero, most of the time no lines are drawn at all. I did once see sensible values for stateEstimate, but those went away again shortly after I started flying.
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:
When I log goodCnt and badCnt, goodCnt increases steadily and badCnt remains at or near zero, so it seems that observations are being enqueued for the estimator.
Looking at the boot log, it seems that everything starts up OK and the Kalman estimator is selected:
Do you have any ideas about what the problem might be here? Thanks.
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