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

[WIP] manual_control selector hacks

Browse Source
This commit is contained in:
Daniel Agar
2021-11-09 16:05:25 +01:00
committed by Matthias Grob
parent e18cf3da3e
commit 2d816e0b3e
54 changed files with 574 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rc_update/rc_update.cpp
+32 -32
View File
@@ -526,7 +526,7 @@ void RCUpdate::Run()
}
// limit processing if there's no update
if (rc_updated || (hrt_elapsed_time(&_last_manual_control_setpoint_publish) > 300_ms)) {
if (rc_updated || (hrt_elapsed_time(&_last_manual_control_input_publish) > 300_ms)) {
UpdateManualSetpoint(input_rc.timestamp_last_signal);
}
@@ -661,43 +661,43 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
void RCUpdate::UpdateManualSetpoint(const hrt_abstime &timestamp_sample)
{
manual_control_setpoint_s manual_control_setpoint{};
manual_control_setpoint.timestamp_sample = timestamp_sample;
manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_RC;
manual_control_input_s manual_control_input{};
manual_control_input.timestamp_sample = timestamp_sample;
manual_control_input.data_source = manual_control_input_s::SOURCE_RC;
// limit controls
manual_control_setpoint.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f);
manual_control_setpoint.x = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f);
manual_control_setpoint.r = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f);
manual_control_setpoint.z = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, -1.f, 1.f);
manual_control_setpoint.flaps = get_rc_value(rc_channels_s::FUNCTION_FLAPS, -1.f, 1.f);
manual_control_setpoint.aux1 = get_rc_value(rc_channels_s::FUNCTION_AUX_1, -1.f, 1.f);
manual_control_setpoint.aux2 = get_rc_value(rc_channels_s::FUNCTION_AUX_2, -1.f, 1.f);
manual_control_setpoint.aux3 = get_rc_value(rc_channels_s::FUNCTION_AUX_3, -1.f, 1.f);
manual_control_setpoint.aux4 = get_rc_value(rc_channels_s::FUNCTION_AUX_4, -1.f, 1.f);
manual_control_setpoint.aux5 = get_rc_value(rc_channels_s::FUNCTION_AUX_5, -1.f, 1.f);
manual_control_setpoint.aux6 = get_rc_value(rc_channels_s::FUNCTION_AUX_6, -1.f, 1.f);
manual_control_input.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f);
manual_control_input.x = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f);
manual_control_input.r = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f);
manual_control_input.z = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, -1.f, 1.f);
manual_control_input.flaps = get_rc_value(rc_channels_s::FUNCTION_FLAPS, -1.f, 1.f);
manual_control_input.aux1 = get_rc_value(rc_channels_s::FUNCTION_AUX_1, -1.f, 1.f);
manual_control_input.aux2 = get_rc_value(rc_channels_s::FUNCTION_AUX_2, -1.f, 1.f);
manual_control_input.aux3 = get_rc_value(rc_channels_s::FUNCTION_AUX_3, -1.f, 1.f);
manual_control_input.aux4 = get_rc_value(rc_channels_s::FUNCTION_AUX_4, -1.f, 1.f);
manual_control_input.aux5 = get_rc_value(rc_channels_s::FUNCTION_AUX_5, -1.f, 1.f);
manual_control_input.aux6 = get_rc_value(rc_channels_s::FUNCTION_AUX_6, -1.f, 1.f);
// publish manual_control_setpoint topic
manual_control_setpoint.timestamp = hrt_absolute_time();
_manual_control_setpoint_pub.publish(manual_control_setpoint);
_last_manual_control_setpoint_publish = manual_control_setpoint.timestamp;
// publish manual_control_input topic
manual_control_input.timestamp = hrt_absolute_time();
_manual_control_input_pub.publish(manual_control_input);
_last_manual_control_input_publish = manual_control_input.timestamp;
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;
// populate and publish actuator_controls_3 copied from mapped manual_control_input
actuator_group_3.control[0] = manual_control_input.y;
actuator_group_3.control[1] = manual_control_input.x;
actuator_group_3.control[2] = manual_control_input.r;
actuator_group_3.control[3] = manual_control_input.z;
actuator_group_3.control[4] = manual_control_input.flaps;
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;
new_aux_values[0] = manual_control_input.aux1;
new_aux_values[1] = manual_control_input.aux2;
new_aux_values[2] = manual_control_input.aux3;
// if AUX RC was already active, we update. otherwise, we check
// if there is a major stick movement to re-activate RC mode
@@ -705,7 +705,7 @@ void RCUpdate::UpdateManualSetpoint(const hrt_abstime &timestamp_sample)
// 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) {
if (fabsf(_last_manual_control_input[i] - new_aux_values[i]) > 0.1f) {
major_movement[i] = true;
}
}
@@ -713,14 +713,14 @@ void RCUpdate::UpdateManualSetpoint(const hrt_abstime &timestamp_sample)
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])
if ((fabsf(_last_manual_control_input[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];
_last_manual_control_input[i] = new_aux_values[i];
actuator_group_3.control[5 + i] = new_aux_values[i];
}
@@ -789,7 +789,7 @@ int RCUpdate::print_usage(const char *reason)
### Description
The rc_update module handles RC channel mapping: read the raw input channels (`input_rc`),
then apply the calibration, map the RC channels to the configured channels & mode switches
and then publish as `rc_channels` and `manual_control_setpoint`.
and then publish as `rc_channels` and `manual_control_input`.
### Implementation
To reduce control latency, the module is scheduled on input_rc publications.
src/modules/rc_update/rc_update.h
+4 -4
View File
@@ -53,7 +53,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/rc_channels.h>
@@ -166,7 +166,7 @@ private:
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)};
uORB::PublicationMulti<manual_control_input_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
uORB::Publication<manual_control_switches_s> _manual_control_switches_pub{ORB_ID(manual_control_switches)};
uORB::Publication<actuator_controls_s> _actuator_group_3_pub{ORB_ID(actuator_controls_3)};
@@ -177,12 +177,12 @@ private:
rc_parameter_map_s _rc_parameter_map {};
float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN] {}; /**< parameter values for RC control */
hrt_abstime _last_manual_control_setpoint_publish{0};
hrt_abstime _last_manual_control_input_publish{0};
hrt_abstime _last_rc_to_param_map_time{0};
hrt_abstime _last_timestamp_signal{0};
uint16_t _rc_values_previous[RC_MAX_CHAN_COUNT] {};
float _last_manual_control_setpoint[3] {};
float _last_manual_control_input[3] {};
bool _aux_already_active[3] = {false, false, false};
uint8_t _channel_count_previous{0};