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

simulator_mavlink: fix esc_status publication

Browse Source
This commit is contained in:
Beat Küng
2022-09-01 17:07:23 -04:00
committed by Daniel Agar
parent 295caaea60
commit f57321d365
1 changed files with 12 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
+12 -4
View File
@@ -224,22 +224,30 @@ void SimulatorMavlink::send_esc_telemetry(mavlink_hil_actuator_controls_t hil_ac
{ {
esc_status_s esc_status{}; esc_status_s esc_status{};
esc_status.timestamp = hrt_absolute_time(); esc_status.timestamp = hrt_absolute_time();
esc_status.esc_count = math::min(actuator_outputs_s::NUM_ACTUATOR_OUTPUTS, esc_status_s::CONNECTED_ESC_MAX); const int max_esc_count = math::min(actuator_outputs_s::NUM_ACTUATOR_OUTPUTS, esc_status_s::CONNECTED_ESC_MAX);
const bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); const bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
esc_status.esc_armed_flags = armed ? 255 : 0; // ugly int max_esc_index = 0;
for (int i = 0; i < max_esc_count; i++) {
if (_output_functions[i] != 0) {
max_esc_index = i;
}
for (int i = 0; i < esc_status.esc_count; i++) {
esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_sim... esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_sim...
esc_status.esc[i].timestamp = esc_status.timestamp; esc_status.esc[i].timestamp = esc_status.timestamp;
esc_status.esc[i].esc_errorcount = 0; // TODO esc_status.esc[i].esc_errorcount = 0; // TODO
esc_status.esc[i].esc_voltage = _battery_status.voltage_v; esc_status.esc[i].esc_voltage = _battery_status.voltage_v;
esc_status.esc[i].esc_current = armed ? 1.0 + math::abs_t(hil_act_control.controls[i]) * 15.0 : esc_status.esc[i].esc_current = armed ? 1.0f + math::abs_t(hil_act_control.controls[i]) * 15.0f :
0.0f; // TODO: magic number 0.0f; // TODO: magic number
esc_status.esc[i].esc_rpm = hil_act_control.controls[i] * 6000; // TODO: magic number esc_status.esc[i].esc_rpm = hil_act_control.controls[i] * 6000; // TODO: magic number
esc_status.esc[i].esc_temperature = 20.0 + math::abs_t(hil_act_control.controls[i]) * 40.0; esc_status.esc[i].esc_temperature = 20.0 + math::abs_t(hil_act_control.controls[i]) * 40.0;
} }
esc_status.esc_count = max_esc_index + 1;
esc_status.esc_armed_flags = (1u << esc_status.esc_count) - 1;
esc_status.esc_online_flags = (1u << esc_status.esc_count) - 1;
_esc_status_pub.publish(esc_status); _esc_status_pub.publish(esc_status);
} }