Skip to content

Commit

Permalink
Move publishing EscStatus from HIL_ACTUATOR_CONTROLS stream into PWMSim
Browse files Browse the repository at this point in the history 
The esc status information is read and published by the esc driver, which is in simulation
case the PWMSim module. Mavlink stream sending is a wrong place to do that.

Signed-off-by: Jukka Laitinen <[email protected]>
  • Loading branch information
@jlaitine
jlaitine committed Aug 22, 2024
1 parent 3e701f7 commit aac3a53
Show file tree
Hide file tree
Showing 3 changed files with 48 additions and 45 deletions.
43 changes: 0 additions & 43 deletions src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,6 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/esc_status.h>

class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams
{
Expand All @@ -62,51 +61,11 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams
ModuleParams(nullptr)
{
_act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};

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]);
}
}

uORB::Subscription _act_sub{ORB_ID(actuator_outputs)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};

int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};

void send_esc_telemetry(mavlink_hil_actuator_controls_t &hil_act_control, vehicle_status_s &vehicle_status)
{
esc_status_s esc_status{};
esc_status.timestamp = hrt_absolute_time();
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);
int max_esc_index = 0;

for (int i = 0; i < max_esc_count; i++) {
if (_output_functions[i] != 0) {
max_esc_index = i;
}

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].esc_errorcount = 0; // TODO
esc_status.esc[i].esc_voltage = 11.1f + math::abs_t(hil_act_control.controls[i]) * 3.0f; // TODO: magic number
esc_status.esc[i].esc_current = armed ? 1.0f + math::abs_t(hil_act_control.controls[i]) * 15.0f :
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.0f + math::abs_t(hil_act_control.controls[i]) * 40.0f;
}

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);
}

bool send() override
{
Expand Down Expand Up @@ -159,8 +118,6 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams

mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg);

send_esc_telemetry(msg, status);

return true;
}

Expand Down
43 changes: 42 additions & 1 deletion src/modules/simulation/pwm_out_sim/PWMSim.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,12 @@
PWMSim::PWMSim(bool hil_mode_enabled) :
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
{
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]);
}

_mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC);
_mixing_output.setAllFailsafeValues(PWM_SIM_FAILSAFE_MAGIC);
_mixing_output.setAllMinValues(PWM_SIM_PWM_MIN_MAGIC);
Expand All @@ -58,13 +64,41 @@ PWMSim::~PWMSim()
perf_free(_interval_perf);
}

void PWMSim::send_esc_telemetry(hrt_abstime ts, float actuator_outputs[MAX_ACTUATORS],
bool actuator_armed[MAX_ACTUATORS], unsigned num_outputs)
{
esc_status_s esc_status{};

if (num_outputs > esc_status_s::CONNECTED_ESC_MAX) {
num_outputs = esc_status_s::CONNECTED_ESC_MAX;
}

for (unsigned i = 0; i < num_outputs; i++) {
esc_status.esc[i].actuator_function = _output_functions[i];
esc_status.esc[i].timestamp = ts;
esc_status.esc[i].esc_errorcount = 0; // TODO
esc_status.esc[i].esc_voltage = 11.1f + math::abs_t(actuator_outputs[i]) * 3.0f; // TODO: magic number
esc_status.esc[i].esc_current = actuator_armed[i] ? 1.0f + math::abs_t(actuator_outputs[i]) * 15.0f :
0.0f; // TODO: magic number
esc_status.esc[i].esc_rpm = actuator_outputs[i] * 6000; // TODO: magic number
esc_status.esc[i].esc_temperature = 20.0f + math::abs_t(actuator_outputs[i]) * 40.0f;
}

esc_status.esc_count = num_outputs;
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);
}

bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
// Only publish once we receive actuator_controls (important for lock-step to work correctly)
if (num_control_groups_updated > 0) {
actuator_outputs_s actuator_outputs{};
actuator_outputs.noutputs = num_outputs;
bool actuator_armed[MAX_ACTUATORS] {};

const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();

Expand All @@ -75,6 +109,8 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], un
bool is_reversible = reversible_outputs & (1u << i);
float output = outputs[i];

actuator_armed[i] = true;

if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)
&& !is_reversible) {
// Scale non-reversible motors to [0, 1]
Expand All @@ -89,8 +125,13 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], un
}
}

actuator_outputs.timestamp = hrt_absolute_time();
hrt_abstime now = hrt_absolute_time();
actuator_outputs.timestamp = now;
_actuator_outputs_sim_pub.publish(actuator_outputs);

// TODO: Add an option to receive ESC telemetry from gazebo via Mavlink and skip this one
send_esc_telemetry(now, actuator_outputs.output, actuator_armed, num_outputs);

return true;
}

Expand Down
7 changes: 6 additions & 1 deletion src/modules/simulation/pwm_out_sim/PWMSim.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
#include <px4_platform_common/time.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/esc_status.h>

#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
#define PARAM_PREFIX "PWM_MAIN"
Expand Down Expand Up @@ -76,6 +77,9 @@ class PWMSim : public ModuleBase<PWMSim>, public OutputModuleInterface
private:
void Run() override;

void send_esc_telemetry(hrt_abstime now, float actuator_outputs[MAX_ACTUATORS], bool actuator_armed[MAX_ACTUATORS],
unsigned num_outputs);

static constexpr uint16_t PWM_SIM_DISARMED_MAGIC = 900;
static constexpr uint16_t PWM_SIM_FAILSAFE_MAGIC = 600;
static constexpr uint16_t PWM_SIM_PWM_MIN_MAGIC = 1000;
Expand All @@ -85,7 +89,8 @@ class PWMSim : public ModuleBase<PWMSim>, public OutputModuleInterface
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

uORB::Publication<actuator_outputs_s> _actuator_outputs_sim_pub{ORB_ID(actuator_outputs_sim)};

uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
};

0 comments on commit aac3a53

Please sign in to comment.