AP_PiccoloCAN: send incomming servo telem data to new `AP_Servo_Telem` lib

This commit is contained in:
Iampete1 2024-11-16 11:12:08 +00:00 committed by Andrew Tridgell
parent d5c29735d6
commit 71137dac31
2 changed files with 26 additions and 31 deletions

View File

@ -31,10 +31,10 @@
#include <AP_HAL/utility/sparse-endian.h> #include <AP_HAL/utility/sparse-endian.h>
#include <SRV_Channel/SRV_Channel.h> #include <SRV_Channel/SRV_Channel.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_CANManager/AP_CANManager.h> #include <AP_CANManager/AP_CANManager.h>
#include <AP_EFI/AP_EFI_Currawong_ECU.h> #include <AP_EFI/AP_EFI_Currawong_ECU.h>
#include <AP_Servo_Telem/AP_Servo_Telem.h>
#include <stdio.h> #include <stdio.h>
@ -320,8 +320,6 @@ bool AP_PiccoloCAN::read_frame(AP_HAL::CANFrame &recv_frame, uint32_t timeout_us
// called from SRV_Channels // called from SRV_Channels
void AP_PiccoloCAN::update() void AP_PiccoloCAN::update()
{ {
uint64_t timestamp = AP_HAL::micros64();
/* Read out the servo commands from the channel mixer */ /* Read out the servo commands from the channel mixer */
for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) { for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) {
@ -361,46 +359,44 @@ void AP_PiccoloCAN::update()
} }
#endif // AP_EFI_CURRAWONG_ECU_ENABLED #endif // AP_EFI_CURRAWONG_ECU_ENABLED
#if HAL_LOGGING_ENABLED #if AP_SERVO_TELEM_ENABLED
AP_Logger *logger = AP_Logger::get_singleton(); AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
if (servo_telem != nullptr) {
// Push received telemetry data into the logging system
if (logger && logger->logging_enabled()) {
WITH_SEMAPHORE(_telem_sem);
for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) { for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) {
AP_PiccoloCAN_Servo &servo = _servos[ii]; AP_PiccoloCAN_Servo &servo = _servos[ii];
if (servo.newTelemetry) { if (servo.newTelemetry) {
union { union {
Servo_ErrorBits_t ebits; Servo_ErrorBits_t ebits;
uint8_t errors; uint8_t errors;
} err; } err;
err.ebits = servo.status.statusA.errors; err.ebits = servo.status.statusA.errors;
logger->Write_ServoStatus(
timestamp, const AP_Servo_Telem::TelemetryData telem_data {
ii, .command_position = servo.commandedPosition(),
servo.position(), // Servo position (represented in microsecond units) .measured_position = servo.position(),
servo.current() * 0.01f, // Servo force (actually servo current, 0.01A per bit) .speed = servo.speed(),
servo.speed(), // Servo speed (degrees per second) .voltage = servo.voltage(),
servo.dutyCycle(), // Servo duty cycle (absolute value as it can be +/- 100%) .current = servo.current(),
uint16_t(servo.commandedPosition()), // Commanded position .duty_cycle = servo.dutyCycle(),
servo.voltage(), // Servo voltage .motor_temperature_cdeg = int16_t(servo.temperature() * 100),
servo.current(), // Servo current .status_flags = err.errors,
servo.temperature(), // Servo temperature .valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
servo.temperature(), // AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
err.errors AP_Servo_Telem::TelemetryData::Types::SPEED |
); AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
AP_Servo_Telem::TelemetryData::Types::CURRENT |
AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE |
AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP |
AP_Servo_Telem::TelemetryData::Types::STATUS
};
servo_telem->update_telem_data(ii, telem_data);
servo.newTelemetry = false; servo.newTelemetry = false;
} }
} }
} }
#else #endif
(void)timestamp;
#endif // HAL_LOGGING_ENABLED
} }

View File

@ -131,7 +131,6 @@ private:
AP_Int16 _ecu_id; //!< ECU Node ID AP_Int16 _ecu_id; //!< ECU Node ID
AP_Int16 _ecu_hz; //!< ECU update rate (Hz) AP_Int16 _ecu_hz; //!< ECU update rate (Hz)
HAL_Semaphore _telem_sem;
}; };
#endif // HAL_PICCOLO_CAN_ENABLE #endif // HAL_PICCOLO_CAN_ENABLE