mirror of https://github.com/ArduPilot/ardupilot
AP_PiccoloCAN: send incomming servo telem data to new `AP_Servo_Telem` lib
This commit is contained in:
parent
d5c29735d6
commit
71137dac31
|
@ -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
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue