diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 8cdd807672..0dd34d2fc4 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -56,9 +56,12 @@ #include #include #include +#include #include #include #include +#include +#include #include #include #include @@ -190,6 +193,7 @@ private: uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::PublicationMulti _flow_pub{ORB_ID(optical_flow)}; uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; + uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; @@ -232,6 +236,7 @@ private: void send(); void send_controls(); void send_heartbeat(); + void send_esc_telemetry(mavlink_hil_actuator_controls_t hil_act_control); void send_mavlink_message(const mavlink_message_t &aMsg); void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors); @@ -259,12 +264,14 @@ private: uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; // hil map_ref data MapProjection _global_local_proj_ref{}; float _global_local_alt0{NAN}; vehicle_status_s _vehicle_status{}; + battery_status_s _battery_status{}; bool _accel_blocked[ACCEL_COUNT_MAX] {}; bool _accel_stuck[ACCEL_COUNT_MAX] {}; @@ -293,6 +300,8 @@ private: float _last_baro_pressure{0.0f}; float _last_baro_temperature{0.0f}; + int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {}; + #if defined(ENABLE_LOCKSTEP_SCHEDULER) px4::atomic _has_initialized {false}; #endif diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index d7eebd68ff..f78a7db9b7 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -87,6 +87,12 @@ Simulator::Simulator() int32_t sys_ctrl_alloc = 0; param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc); _use_dynamic_mixing = sys_ctrl_alloc >= 1; + + for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) { + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_%s%d", "PWM_MAIN", "FUNC", i + 1); + param_get(param_find(param_name), &_output_functions[i]); + } } void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg) @@ -200,6 +206,29 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t * #endif } +void Simulator::send_esc_telemetry(mavlink_hil_actuator_controls_t hil_act_control) +{ + esc_status_s esc_status{}; + 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 bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + esc_status.esc_armed_flags = armed ? 255 : 0; // ugly + + 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_out_sim... + esc_status.esc[i].timestamp = esc_status.timestamp; + esc_status.esc[i].esc_errorcount = 0; // TODO + 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 : + 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_temperature = 20.0 + math::abs_t(hil_act_control.controls[i]) * 40.0; + } + + _esc_status_pub.publish(esc_status); +} + void Simulator::send_controls() { orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs); @@ -214,6 +243,8 @@ void Simulator::send_controls() PX4_DEBUG("sending controls t=%ld (%ld)", _actuator_outputs.timestamp, hil_act_control.time_usec); send_mavlink_message(message); + + send_esc_telemetry(hil_act_control); } } @@ -756,6 +787,7 @@ void Simulator::send() parameters_update(false); check_failure_injections(); _vehicle_status_sub.update(&_vehicle_status); + _battery_status_sub.update(&_battery_status); // Wait for other modules, such as logger or ekf2 px4_lockstep_wait_for_components();