diff --git a/libraries/AP_Gripper/AP_Gripper_EPM.cpp b/libraries/AP_Gripper/AP_Gripper_EPM.cpp index 65dafb272a..becb3f2e8b 100644 --- a/libraries/AP_Gripper/AP_Gripper_EPM.cpp +++ b/libraries/AP_Gripper/AP_Gripper_EPM.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #ifdef UAVCAN_NODE_FILE #include #include @@ -60,6 +61,7 @@ void AP_Gripper_EPM::grab() SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm); } gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing"); + AP::logger().Write_Event(LogEvent::GRIPPER_GRAB); } // release - move epm pwm output to the release position @@ -83,6 +85,7 @@ void AP_Gripper_EPM::release() SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm); } gcs().send_text(MAV_SEVERITY_INFO, "Gripper load releasing"); + AP::logger().Write_Event(LogEvent::GRIPPER_RELEASE); } // neutral - return the epm pwm output to the neutral position diff --git a/libraries/AP_Gripper/AP_Gripper_Servo.cpp b/libraries/AP_Gripper/AP_Gripper_Servo.cpp index 565774b623..9747222110 100644 --- a/libraries/AP_Gripper/AP_Gripper_Servo.cpp +++ b/libraries/AP_Gripper/AP_Gripper_Servo.cpp @@ -1,5 +1,7 @@ #include #include +#include + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include #endif @@ -22,6 +24,7 @@ void AP_Gripper_Servo::grab() is_released = true; #endif gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing"); + AP::logger().Write_Event(LogEvent::GRIPPER_GRAB); } void AP_Gripper_Servo::release() @@ -34,6 +37,7 @@ void AP_Gripper_Servo::release() is_released = false; #endif gcs().send_text(MAV_SEVERITY_INFO, "Gripper load releasing"); + AP::logger().Write_Event(LogEvent::GRIPPER_RELEASE); } bool AP_Gripper_Servo::has_state_pwm(const uint16_t pwm) const