From 659885419aa4e74e6b1adaf6bf987d81a0526b16 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Dec 2022 14:17:00 +1100 Subject: [PATCH] AP_PiccoloCAN: expand servo status logging --- libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index 339d5a7205..ca3f6c4d0d 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -382,14 +382,24 @@ void AP_PiccoloCAN::update() CBSServo_Info_t &servo = _servo_info[ii]; if (servo.newTelemetry) { - + union { + Servo_ErrorBits_t ebits; + uint8_t errors; + } err; + err.ebits = servo.statusA.errors; logger->Write_ServoStatus( timestamp, ii, (float) servo.statusA.position, // Servo position (represented in microsecond units) (float) servo.statusB.current * 0.01f, // Servo force (actually servo current, 0.01A per bit) (float) servo.statusB.speed, // Servo speed (degrees per second) - (uint8_t) abs(servo.statusB.dutyCycle) // Servo duty cycle (absolute value as it can be +/- 100%) + (uint8_t) abs(servo.statusB.dutyCycle), // Servo duty cycle (absolute value as it can be +/- 100%) + servo.statusA.command, + servo.statusB.voltage*0.01, + servo.statusB.current*0.01, + servo.statusB.temperature, + servo.statusB.temperature, + err.errors ); servo.newTelemetry = false;