fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 23:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

actuator: add support for MAV_CMD_DO_SET_ACTUATOR

Browse Source 
Adds support for using the MAVLink command MAV_CMD_DO_SET_ACTUATOR to
update the actuator values on control group 3 aux{1, 2, 3}. A simple
deconfliction with rc_update is implemented: when a MAVLink command is
sent, RC is disabled for that channel until a major stick movement is
detected.
This commit is contained in:
Kalyan Sriram
2021-03-02 12:41:02 +01:00
committed by Lorenz Meier
parent ada05165f1
commit b257f9d1fd
4 changed files with 71 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rc_update/rc_update.cpp
+35 -4
View File
@@ -602,16 +602,47 @@ void RCUpdate::UpdateManualSetpoint(const hrt_abstime &timestamp_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);
src/modules/rc_update/rc_update.h
+3
View File
@@ -161,6 +161,7 @@ private:
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_s> _rc_channels_pub{ORB_ID(rc_channels)};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
@@ -179,6 +180,8 @@ private:
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};