Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_ServoTelem: added active mask 

and fixed typo in logging
This commit is contained in:
Andrew Tridgell 2024-11-19 08:19:58 +11:00
parent bf5555ccd3
commit 445c03c69c
2 changed files with 12 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Servo_Telem/AP_Servo_Telem.cpp
View File

@ -61,6 +61,7 @@ void AP_Servo_Telem::update_telem_data(const uint8_t servo_index, const Telemetr
if (servo_index >= ARRAY_SIZE(_telem_data) || (new_data.valid_types == 0)) { if (servo_index >= ARRAY_SIZE(_telem_data) || (new_data.valid_types == 0)) {
return; return;
} }
active_mask |= 1U << servo_index;
volatile TelemetryData &telemdata = _telem_data[servo_index]; volatile TelemetryData &telemdata = _telem_data[servo_index];
@ -112,7 +113,7 @@ void AP_Servo_Telem::write_log()
AP_Logger *logger = AP_Logger::get_singleton(); AP_Logger *logger = AP_Logger::get_singleton();
// Check logging is available and enabled // Check logging is available and enabled
if ((logger == nullptr) || !logger->logging_enabled()) { if ((logger == nullptr) || !logger->logging_enabled() || active_mask == 0) {
return; return;
} }
@ -134,15 +135,15 @@ void AP_Servo_Telem::write_log()
LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG), LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG),
time_us : now_us, time_us : now_us,
id : i, id : i,
position : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.measured_position : AP::logger().quiet_nanf(), position : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.measured_position : AP::logger().quiet_nanf(),
force : telemdata.present(TelemetryData::Types::FORCE) ? telemdata.force : AP::logger().quiet_nanf(), force : telemdata.present(TelemetryData::Types::FORCE) ? telemdata.force : AP::logger().quiet_nanf(),
speed : telemdata.present(TelemetryData::Types::SPEED) ? telemdata.speed : AP::logger().quiet_nanf(), speed : telemdata.present(TelemetryData::Types::SPEED) ? telemdata.speed : AP::logger().quiet_nanf(),
power_pct : telemdata.duty_cycle, power_pct : telemdata.duty_cycle,
pos_cmd : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.command_position : AP::logger().quiet_nanf(), pos_cmd : telemdata.present(TelemetryData::Types::COMMANDED_POSITION) ? telemdata.command_position : AP::logger().quiet_nanf(),
voltage : telemdata.present(TelemetryData::Types::VOLTAGE) ? telemdata.voltage : AP::logger().quiet_nanf(), voltage : telemdata.present(TelemetryData::Types::VOLTAGE) ? telemdata.voltage : AP::logger().quiet_nanf(),
current : telemdata.present(TelemetryData::Types::CURRENT) ? telemdata.current : AP::logger().quiet_nanf(), current : telemdata.present(TelemetryData::Types::CURRENT) ? telemdata.current : AP::logger().quiet_nanf(),
mot_temp : telemdata.present(TelemetryData::Types::MOTOR_TEMP) ? telemdata.motor_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(), mot_temp : telemdata.present(TelemetryData::Types::MOTOR_TEMP) ? telemdata.motor_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(),
pcb_temp : telemdata.present(TelemetryData::Types::PCB_TEMP) ? telemdata.pcb_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(), pcb_temp : telemdata.present(TelemetryData::Types::PCB_TEMP) ? telemdata.pcb_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(),
error : telemdata.status_flags, error : telemdata.status_flags,
}; };
AP::logger().WriteBlock(&pkt, sizeof(pkt)); AP::logger().WriteBlock(&pkt, sizeof(pkt));

2
libraries/AP_Servo_Telem/AP_Servo_Telem.h
View File

@ -80,5 +80,7 @@ private:
uint32_t _last_telem_log_ms[SERVO_TELEM_MAX_SERVOS]; uint32_t _last_telem_log_ms[SERVO_TELEM_MAX_SERVOS];
static AP_Servo_Telem *_singleton; static AP_Servo_Telem *_singleton;
uint32_t active_mask;
}; };
#endif // AP_SERVO_TELEM_ENABLED #endif // AP_SERVO_TELEM_ENABLED