diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index c3eeec3248..75f115d2c8 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -169,15 +169,14 @@ bool_t navdata_init() uint8_t cmd=0x02; navdata_write(&cmd, 1); - // read some potential dirt (retry alot of times) + // read some potential dirt + // wait 10 milliseconds char tmp[100]; - for(int i = 0; i < 100; i++) { + for(int i = 0; i < 12; i++) { uint16_t dirt = read(nav_fd, tmp, sizeof tmp); (void) dirt; - cmd=0x02; - navdata_write(&cmd, 1); - usleep(200); + usleep(1000); } baro_calibrated = FALSE; @@ -213,6 +212,12 @@ static inline bool_t acquire_baro_calibration(void) uint8_t cmd=0x17; // send cmd 23 navdata_write(&cmd, 1); + // wait 20ms to retrieve data + for (int i=0;i<22;i++) + { + usleep(1000); + } + uint8_t calibBuffer[22]; if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0)