diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index 9dc1ab1619..fd2ab2c58b 100644 --- a/libraries/AP_EFI/AP_EFI.cpp +++ b/libraries/AP_EFI/AP_EFI.cpp @@ -86,7 +86,7 @@ void AP_EFI::init(void) #endif break; default: - gcs().send_text(MAV_SEVERITY_INFO, "Unknown EFI type"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unknown EFI type"); break; } } @@ -96,7 +96,9 @@ void AP_EFI::update() { if (backend) { backend->update(); +#if HAL_LOGGING_ENABLED log_status(); +#endif } } @@ -105,6 +107,7 @@ bool AP_EFI::is_healthy(void) const return (backend && (AP_HAL::millis() - state.last_updated_ms) < HEALTHY_LAST_RECEIVED_MS); } +#if HAL_LOGGING_ENABLED /* write status to log */ @@ -208,6 +211,7 @@ void AP_EFI::log_status(void) state.ecu_index); } } +#endif // LOGGING_ENABLED /* send EFI_STATUS @@ -240,6 +244,13 @@ void AP_EFI::send_mavlink_status(mavlink_channel_t chan) ); } +// get a copy of state structure +void AP_EFI::get_state(EFI_State &_state) +{ + WITH_SEMAPHORE(sem); + _state = state; +} + namespace AP { AP_EFI *EFI() { diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h index fb5eef065c..36a97a9cbb 100644 --- a/libraries/AP_EFI/AP_EFI.h +++ b/libraries/AP_EFI/AP_EFI.h @@ -64,6 +64,14 @@ public: bool is_healthy() const; + // return timestamp of last update + uint32_t get_last_update_ms(void) const { + return state.last_updated_ms; + } + + // get a copy of state structure + void get_state(EFI_State &state); + // Parameter info static const struct AP_Param::GroupInfo var_info[]; @@ -99,6 +107,9 @@ private: AP_EFI_Backend *backend; static AP_EFI *singleton; + // Semaphore for access to shared frontend data + HAL_Semaphore sem; + // write to log void log_status(); }; diff --git a/libraries/AP_EFI/AP_EFI_Backend.cpp b/libraries/AP_EFI/AP_EFI_Backend.cpp index 944ed76bc9..7348c3f69f 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.cpp +++ b/libraries/AP_EFI/AP_EFI_Backend.cpp @@ -28,7 +28,7 @@ AP_EFI_Backend::AP_EFI_Backend(AP_EFI &_frontend) : void AP_EFI_Backend::copy_to_frontend() { - WITH_SEMAPHORE(sem); + WITH_SEMAPHORE(frontend.sem); frontend.state = internal_state; } diff --git a/libraries/AP_EFI/AP_EFI_Backend.h b/libraries/AP_EFI/AP_EFI_Backend.h index b23ab824d6..5c70d4908a 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.h +++ b/libraries/AP_EFI/AP_EFI_Backend.h @@ -35,9 +35,6 @@ protected: // Copies internal state to the frontend state void copy_to_frontend(); - // Semaphore for access to shared frontend data - HAL_Semaphore sem; - // Internal state for this driver (before copying to frontend) EFI_State internal_state; diff --git a/libraries/AP_EFI/AP_EFI_NWPMU.cpp b/libraries/AP_EFI/AP_EFI_NWPMU.cpp index 00bc9d33d3..be69fea4ea 100644 --- a/libraries/AP_EFI/AP_EFI_NWPMU.cpp +++ b/libraries/AP_EFI/AP_EFI_NWPMU.cpp @@ -36,7 +36,7 @@ void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame) { const uint32_t id = frame.id & AP_HAL::CANFrame::MaskExtID; - WITH_SEMAPHORE(sem); + WITH_SEMAPHORE(frontend.sem); switch ((NWPMU_ID)id) { case NWPMU_ID::ECU_1: {