diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 4bb18ad360..b6e7543ec0 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -712,6 +712,7 @@ void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_ backend->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg); } +#if HAL_LOGGING_ENABLED // write mount log packet for all backends void AP_Mount::write_log() { @@ -731,6 +732,7 @@ void AP_Mount::write_log(uint8_t instance, uint64_t timestamp_us) } backend->write_log(timestamp_us); } +#endif // point at system ID sysid void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index e9760c7ae0..28536134ad 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -409,6 +409,7 @@ bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavli return true; } +#if HAL_LOGGING_ENABLED // write mount log packet void AP_Mount_Backend::write_log(uint64_t timestamp_us) { @@ -456,6 +457,7 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us) }; AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt)); } +#endif #if AP_MOUNT_POI_TO_LATLONALT_ENABLED // get poi information. Returns true on success and fills in gimbal attitude, location and poi location diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 6dbf8cdbd3..b188f945a2 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -125,19 +125,26 @@ void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, const mav _gimbal.update_target(Vector3f{mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw()}); _gimbal.receive_feedback(chan,msg); +#if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); if (logger == nullptr) { return; } - - if(!_params_saved && logger->logging_started()) { +#endif + if(!_params_saved +#if HAL_LOGGING_ENABLED + && logger->logging_started() +#endif + ) { _gimbal.fetch_params(); //last parameter save might not be stored in logger so retry _params_saved = true; } +#if HAL_LOGGING_ENABLED if (_gimbal.get_log_dt() > 1.0f/25.0f) { _gimbal.write_logs(); } +#endif } void AP_Mount_SoloGimbal::handle_param_value(const mavlink_message_t &msg) diff --git a/libraries/AP_Mount/SoloGimbal.cpp b/libraries/AP_Mount/SoloGimbal.cpp index 01b8aec829..4503c02cec 100644 --- a/libraries/AP_Mount/SoloGimbal.cpp +++ b/libraries/AP_Mount/SoloGimbal.cpp @@ -400,6 +400,7 @@ void SoloGimbal::update_target(const Vector3f &newTarget) _att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x,radians(-30),radians(30)); } +#if HAL_LOGGING_ENABLED void SoloGimbal::write_logs() { AP_Logger &logger = AP::logger(); @@ -481,6 +482,7 @@ void SoloGimbal::write_logs() _log_del_ang.zero(); _log_del_vel.zero(); } +#endif bool SoloGimbal::joints_near_limits() const { diff --git a/libraries/AP_Mount/SoloGimbal_Parameters.cpp b/libraries/AP_Mount/SoloGimbal_Parameters.cpp index 8245086ba3..60bf156acb 100644 --- a/libraries/AP_Mount/SoloGimbal_Parameters.cpp +++ b/libraries/AP_Mount/SoloGimbal_Parameters.cpp @@ -187,10 +187,12 @@ void SoloGimbal_Parameters::handle_param_value(const mavlink_message_t &msg) mavlink_param_value_t packet; mavlink_msg_param_value_decode(&msg, &packet); +#if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); if (logger != nullptr) { logger->Write_Parameter(packet.param_id, packet.param_value); } +#endif for(uint8_t i=0; i