fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-10 05:48:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

UAVCAN: Fix counters and use correct indexing

Browse Source
This commit is contained in:
Alexander Lerach
2026-01-27 11:26:37 +01:00
parent 175c4454f3
commit 1635e1cfc0
3 changed files with 23 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/ServoReport.msg
+2 -1
View File
@@ -8,13 +8,14 @@ uint8 servo_power_rating_pct # 0 - unloaded, 100 - full load
uint8 servo_function # servo output function
uint16 servo_temperature_counter # Incremented when new temperature data is stored
float32 servo_temperature # in kelvin
uint8 servo_temperature_error_flags
uint8 ERROR_FLAG_OVERHEATING = 1
uint8 ERROR_FLAG_OVERCOOLING = 2
uint16 servo_power_counter # Incremented when new power data is stored
float32 servo_voltage # Volts
float32 servo_current # Amps
uint8 servo_power_error_flags
src/drivers/uavcan/actuators/servo.cpp
+19 -13
View File
@@ -50,6 +50,8 @@ UavcanServoController::UavcanServoController(uavcan::INode &node) :
memset(_last_voltage, 0, sizeof(_last_voltage));
memset(_last_current, 0, sizeof(_last_current));
memset(_last_power_error_flag, 0, sizeof(_last_power_error_flag));
memset(_servo_temperature_counter, 0, sizeof(_servo_temperature_counter));
memset(_servo_power_counter, 0, sizeof(_servo_power_counter));
}
int
@@ -120,6 +122,7 @@ UavcanServoController::servo_temperature_sub_cb(const
const bool is_servo_matching = ref.servo_node_id == msg.getSrcNodeID().get();
if (is_servo_online && is_servo_matching) {
_servo_temperature_counter[i] += 1;
_last_temperature[i] = msg.temperature;
_last_temperature_error_flag[i] = msg.error_flags;
break;
@@ -139,9 +142,11 @@ UavcanServoController::servo_circuit_status_sub_cb(const
const bool is_servo_matching = ref.servo_node_id == msg.getSrcNodeID().get();
if (is_servo_online && is_servo_matching) {
_last_voltage[msg.circuit_id] = msg.voltage;
_last_current[msg.circuit_id] = msg.current;
_last_power_error_flag[msg.circuit_id] = msg.error_flags;
_servo_power_counter[i] += 1;
_last_voltage[i] = msg.voltage;
_last_current[i] = msg.current;
_last_power_error_flag[i] = msg.error_flags;
break;
}
}
}
@@ -153,21 +158,23 @@ UavcanServoController::servo_status_sub_cb(const uavcan::ReceivedDataStructure<u
if (msg.actuator_id < servo_status_s::CONNECTED_SERVO_MAX) {
auto &ref = _servo_status.servo[msg.actuator_id];
ref.timestamp = hrt_absolute_time();
ref.servo_node_id = msg.getSrcNodeID().get();
ref.servo_actuator_id = msg.actuator_id;
ref.servo_position = msg.position;
ref.servo_force = msg.force;
ref.servo_speed = msg.speed;
ref.timestamp = hrt_absolute_time();
ref.servo_node_id = msg.getSrcNodeID().get();
ref.servo_actuator_id = msg.actuator_id;
ref.servo_position = msg.position;
ref.servo_force = msg.force;
ref.servo_speed = msg.speed;
ref.servo_power_rating_pct = msg.power_rating_pct;
// Add servo temperature data
ref.servo_temperature = _last_temperature[msg.actuator_id];
ref.servo_temperature_counter = _servo_temperature_counter[msg.actuator_id];
ref.servo_temperature = _last_temperature[msg.actuator_id];
ref.servo_temperature_error_flags = _last_temperature_error_flag[msg.actuator_id];
// Add servo power data
ref.servo_voltage = _last_voltage[msg.actuator_id];
ref.servo_current = _last_current[msg.actuator_id];
ref.servo_power_counter = _servo_power_counter[msg.actuator_id];
ref.servo_voltage = _last_voltage[msg.actuator_id];
ref.servo_current = _last_current[msg.actuator_id];
ref.servo_power_error_flags = _last_power_error_flag[msg.actuator_id];
_servo_status.counter += 1;
@@ -190,7 +197,6 @@ UavcanServoController::check_servos_status()
if (_servo_status.servo[index].timestamp > 0 && now - _servo_status.servo[index].timestamp < 1200_ms) {
servo_status_flags |= (1 << index);
}
}
return servo_status_flags;
src/drivers/uavcan/actuators/servo.hpp
+2 -3
View File
@@ -120,7 +120,6 @@ private:
float _last_voltage[servo_status_s::CONNECTED_SERVO_MAX];
float _last_current[servo_status_s::CONNECTED_SERVO_MAX];
uint8_t _last_power_error_flag[servo_status_s::CONNECTED_SERVO_MAX];
int _servo_temperature_counter{0};
int _servo_power_counter{0};
uint16_t _servo_temperature_counter[servo_status_s::CONNECTED_SERVO_MAX];
uint16_t _servo_power_counter[servo_status_s::CONNECTED_SERVO_MAX];
};