diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f73f8a7b88f2..8682e04dec0d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -489,8 +489,39 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const send_ack = true; } - } else { + } else if (cmd_mavlink.command == MAV_CMD_DO_SET_ACTUATOR) { + // since we're only paying attention to 3 AUX outputs, the + // index should be 0, otherwise ignore the message + if (((int) vehicle_command.param7) == 0) { + actuator_controls_s actuator_controls{}; + actuator_controls.timestamp = hrt_absolute_time(); + + // update with existing values to avoid changing unspecified controls + _actuator_controls_3_sub.update(&actuator_controls); + + bool updated = false; + + if (PX4_ISFINITE(vehicle_command.param1)) { + actuator_controls.control[5] = vehicle_command.param1; + updated = true; + } + + if (PX4_ISFINITE(vehicle_command.param2)) { + actuator_controls.control[6] = vehicle_command.param2; + updated = true; + } + + if (PX4_ISFINITE(vehicle_command.param3)) { + actuator_controls.control[7] = vehicle_command.param3; + updated = true; + } + + if (updated) { + _actuator_controls_pubs[3].publish(actuator_controls); + } + } + } else { send_ack = false; if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 1c8171054cf0..ebe3ce646e92 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -402,6 +402,7 @@ class MavlinkReceiver : public ModuleParams uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index e2a167c65908..017b1e3f6468 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -602,16 +602,47 @@ void RCUpdate::UpdateManualSetpoint(const hrt_abstime ×tamp_sample) _last_manual_control_setpoint_publish = manual_control_setpoint.timestamp; - // populate and publish actuator_controls_3 copied from mapped manual_control_setpoint actuator_controls_s actuator_group_3{}; + // copy in previous actuator control setpoint in case aux{1, 2, 3} isn't changed + _actuator_controls_3_sub.update(&actuator_group_3); + // populate and publish actuator_controls_3 copied from mapped manual_control_setpoint actuator_group_3.control[0] = manual_control_setpoint.y; actuator_group_3.control[1] = manual_control_setpoint.x; actuator_group_3.control[2] = manual_control_setpoint.r; actuator_group_3.control[3] = manual_control_setpoint.z; actuator_group_3.control[4] = manual_control_setpoint.flaps; - actuator_group_3.control[5] = manual_control_setpoint.aux1; - actuator_group_3.control[6] = manual_control_setpoint.aux2; - actuator_group_3.control[7] = manual_control_setpoint.aux3; + + float new_aux_values[3]; + new_aux_values[0] = manual_control_setpoint.aux1; + new_aux_values[1] = manual_control_setpoint.aux2; + new_aux_values[2] = manual_control_setpoint.aux3; + + // if AUX RC was already active, we update. otherwise, we check + // if there is a major stick movement to re-activate RC mode + bool major_movement[3] = {false, false, false}; + + // detect a big stick movement + for (int i = 0; i < 3; i++) { + if (fabsf(_last_manual_control_setpoint[i] - new_aux_values[i]) > 0.1f) { + major_movement[i] = true; + } + } + + for (int i = 0; i < 3; i++) { + // if someone else (DO_SET_ACTUATOR) updated the actuator control + // and we haven't had a major movement, switch back to automatic control + if ((fabsf(_last_manual_control_setpoint[i] - actuator_group_3.control[5 + i]) + > 0.0001f) && (!major_movement[i])) { + _aux_already_active[i] = false; + } + + if (_aux_already_active[i] || major_movement[i]) { + _aux_already_active[i] = true; + _last_manual_control_setpoint[i] = new_aux_values[i]; + + actuator_group_3.control[5 + i] = new_aux_values[i]; + } + } actuator_group_3.timestamp = hrt_absolute_time(); _actuator_group_3_pub.publish(actuator_group_3); diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index 3ee7f290ed70..4e575d6be100 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -161,6 +161,7 @@ class RCUpdate : public ModuleBase, public ModuleParams, public px4::W uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)}; + uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)}; uORB::Publication _rc_channels_pub{ORB_ID(rc_channels)}; uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; @@ -179,6 +180,8 @@ class RCUpdate : public ModuleBase, public ModuleParams, public px4::W hrt_abstime _last_timestamp_signal{0}; uint16_t _rc_values_previous[RC_MAX_CHAN_COUNT] {}; + float _last_manual_control_setpoint[3] {}; + bool _aux_already_active[3] = {false, false, false}; uint8_t _channel_count_previous{0}; uint8_t _input_source_previous{input_rc_s::RC_INPUT_SOURCE_UNKNOWN};