diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index c2bf3cd45a..09f5b3f97d 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -29,7 +29,11 @@ - + @@ -117,7 +121,7 @@
- +
diff --git a/conf/messages.xml b/conf/messages.xml index 5789a130bf..ab52141ce3 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -43,7 +43,7 @@ - + diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index 95d7b6bbf6..314a6f8f63 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -32,20 +32,12 @@ struct Baro baro; -// Over sample setting (0-3) -#define BMP180_OSS 0 - -int32_t pressure; -int32_t temperature; -int32_t pres_raw = 0; -int32_t tmp_raw = 0; -int32_t pr0 = 0; +#define BMP180_OSS 0 // Parrot ARDrone uses no oversampling void baro_init(void) { baro.status = BS_UNINITIALIZED; baro.absolute = 0; baro.differential = 0; - baro_data_available = 0; } static inline int32_t baro_apply_calibration(int32_t raw) @@ -67,36 +59,26 @@ static inline int32_t baro_apply_calibration(int32_t raw) return p + ((x1 + x2 + 3791L) >> 4); } -static inline void baro_read_temp(void) +static inline int32_t baro_apply_calibration_temp(int32_t tmp_raw) { - tmp_raw = (navdata->temperature_pressure & 0x00FF) << 8 | (navdata->temperature_pressure >> 8); int32_t x1 = ((tmp_raw - ((int32_t)baro_calibration.ac6)) * ((int32_t)baro_calibration.ac5)) >> 15; int32_t x2 = (((int32_t)baro_calibration.mc) << 11) / (x1 + ((int32_t)baro_calibration.md)); baro_calibration.b5 = x1 + x2; - temperature = (baro_calibration.b5 + 8) >> 4; + return (baro_calibration.b5 + 8) >> 4; } -static inline void baro_read_pressure(void) +void baro_periodic(void) { - pres_raw = navdata->pressure >> 8; - pres_raw = pres_raw >> (8 - BMP180_OSS); - pressure = baro_apply_calibration(pres_raw); - if ((pr0 != 0) && (pressure != 0)) - pr0 = pressure; - pressure -= pr0; } -void baro_periodic(void) { - if(baro.status == BS_RUNNING && navdata_baro_available == 1) { - navdata_baro_available = 0; - - baro_read_temp(); // first read temperature because pressure calibration depends on temperature - baro_read_pressure(); - baro.absolute = pressure; - baro_data_available = TRUE; +void process_ardrone_baro(void) +{ + if(baro.status == BS_RUNNING) { + // first read temperature because pressure calibration depends on temperature + baro.differential = baro_apply_calibration_temp(navdata->temperature_pressure); // We store the temperature in Baro-Diff + baro.absolute = baro_apply_calibration(navdata->pressure); } else { - baro_data_available = FALSE; if (baro_calibrated == TRUE) { baro.status = BS_RUNNING; } diff --git a/sw/airborne/boards/ardrone/baro_board.h b/sw/airborne/boards/ardrone/baro_board.h index cf76b5e273..d98a771108 100644 --- a/sw/airborne/boards/ardrone/baro_board.h +++ b/sw/airborne/boards/ardrone/baro_board.h @@ -33,10 +33,14 @@ #if BOARD_HAS_BARO #include "navdata.h" -int8_t baro_data_available; +void process_ardrone_baro(void); + static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){ - if (baro_data_available) { + if (navdata_baro_available) + { + navdata_baro_available = 0; + process_ardrone_baro(); b_abs_handler(); } } diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 005bda75cc..831a492367 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -170,7 +170,6 @@ void navdata_read() port->bytesRead += newbytes; port->totalBytesRead += newbytes; } - } void navdata_update() @@ -187,11 +186,24 @@ void navdata_update() // if ( navdata_checksum() == 0 ) { memcpy(navdata, port->buffer, NAVDATA_PACKET_SIZE); + + // Invert byte order so that TELEMETRY works better + uint8_t tmp; + uint8_t* p = (uint8_t*) &(navdata->pressure); + tmp = p[0]; + p[0] = p[1]; + p[1] = tmp; + p = (uint8_t*) &(navdata->temperature_pressure); + tmp = p[0]; + p[0] = p[1]; + p[1] = tmp; + navdata_imu_available = 1; navdata_baro_available = 1; + port->packetsRead++; // printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum)); - navdata_getHeight(); + //navdata_getHeight(); } navdata_CropBuffer(60); } @@ -215,7 +227,7 @@ void navdata_CropBuffer(int cropsize) { if (port->bytesRead - cropsize < 0) { // TODO think about why the amount of bytes read minus the cropsize gets below zero - printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%d\n", port->bytesRead, cropsize, port->buffer); + printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", port->bytesRead, cropsize, port->buffer); return; } diff --git a/sw/airborne/boards/ardrone/navdata.h b/sw/airborne/boards/ardrone/navdata.h index 8c8149c622..ec40241dcc 100644 --- a/sw/airborne/boards/ardrone/navdata.h +++ b/sw/airborne/boards/ardrone/navdata.h @@ -79,7 +79,7 @@ typedef struct uint16_t flag_echo_ini; int32_t pressure; - int16_t temperature_pressure; + uint16_t temperature_pressure; int16_t mx; int16_t my; diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.c b/sw/airborne/subsystems/imu/imu_ardrone2_raw.c index b43a6ab357..a16afa4938 100644 --- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.c +++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.c @@ -30,23 +30,13 @@ #include "mcu_periph/uart.h" void imu_impl_init(void) { - imu_data_available = FALSE; navdata_init(); } void imu_periodic(void) { - navdata_update(); - //checks if the navboard has a new dataset ready - if (navdata_imu_available == TRUE) { - navdata_imu_available = FALSE; - RATES_ASSIGN(imu.gyro_unscaled, navdata->vx, navdata->vy, navdata->vz); - VECT3_ASSIGN(imu.accel_unscaled, navdata->ax, navdata->ay, navdata->az); - VECT3_ASSIGN(imu.mag_unscaled, navdata->mx, navdata->my, navdata->mz); - imu_data_available = TRUE; - } - else { - imu_data_available = FALSE; - } +} + +void navdata_event(void) { #ifdef USE_UART1 uart1_handler(); diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h index ce15a2c2d5..9688459bca 100644 --- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h +++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h @@ -31,16 +31,23 @@ #include "generated/airframe.h" #include "navdata.h" -int imu_data_available; +void navdata_event(void); static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { - if (imu_data_available) { - imu_data_available = FALSE; + navdata_update(); + //checks if the navboard has a new dataset ready + if (navdata_imu_available == TRUE) { + navdata_imu_available = FALSE; + RATES_ASSIGN(imu.gyro_unscaled, navdata->vx, navdata->vy, navdata->vz); + VECT3_ASSIGN(imu.accel_unscaled, navdata->ax, navdata->ay, navdata->az); + VECT3_ASSIGN(imu.mag_unscaled, navdata->mx, navdata->my, navdata->mz); + _gyro_handler(); _accel_handler(); _mag_handler(); } + navdata_event(); } #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \