diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index d940c13295..08878b3c80 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -26,10 +26,6 @@ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 -#include -#endif - #include "DFMessageWriter.h" class DataFlash_Backend; diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 95a89b65f4..f88b296d54 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1512,43 +1512,6 @@ bool DataFlash_Backend::Log_Write_Mode(uint8_t mode, uint8_t reason) // Write ESC status messages void DataFlash_Class::Log_Write_ESC(void) { -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - static int _esc_status_sub = -1; - struct esc_status_s esc_status; - - if (_esc_status_sub == -1) { - // subscribe to ORB topic on first call - _esc_status_sub = orb_subscribe(ORB_ID(esc_status)); - } - - // check for new ESC status data - bool esc_updated = false; - orb_check(_esc_status_sub, &esc_updated); - if (esc_updated && (OK == orb_copy(ORB_ID(esc_status), _esc_status_sub, &esc_status))) { - if (esc_status.esc_count > 8) { - esc_status.esc_count = 8; - } - uint64_t time_us = AP_HAL::micros64(); - for (uint8_t i = 0; i < esc_status.esc_count; i++) { - // skip logging ESCs with a esc_address of zero, and this - // are probably not populated. The Pixhawk itself should - // be address zero - if (esc_status.esc[i].esc_address != 0) { - struct log_Esc pkt = { - LOG_PACKET_HEADER_INIT((uint8_t)(LOG_ESC1_MSG + i)), - time_us : time_us, - rpm : (int32_t)(esc_status.esc[i].esc_rpm/10), - voltage : (uint16_t)(esc_status.esc[i].esc_voltage*100.0f + .5f), - current : (uint16_t)(esc_status.esc[i].esc_current*100.0f + .5f), - temperature : (int16_t)(esc_status.esc[i].esc_temperature*100.0f + .5f), - current_tot : 0 - }; - - WriteBlock(&pkt, sizeof(pkt)); - } - } - } -#endif // CONFIG_HAL_BOARD } // Write a AIRSPEED packet