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

msg: re-use manual_control_input in setpoint

Browse Source 
This way we avoid duplication between manual_control_input and
manual_control_setpoint.
This commit is contained in:
Julian Oes
2021-11-09 16:05:25 +01:00
committed by Matthias Grob
parent baf81abbab
commit 6a6b8d49fc
21 changed files with 125 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/manual_control/ManualControl.cpp
+11 -8
View File
@@ -116,9 +116,12 @@ void ManualControl::Run()
_published_invalid_once = false;
// user arm/disarm gesture
const bool right_stick_centered = (fabsf(_selector.setpoint().x) < 0.1f) && (fabsf(_selector.setpoint().y) < 0.1f);
const bool stick_lower_left = (_selector.setpoint().z < 0.1f) && (_selector.setpoint().r < -0.9f);
const bool stick_lower_right = (_selector.setpoint().z < 0.1f) && (_selector.setpoint().r > 0.9f);
const bool right_stick_centered = (fabsf(_selector.setpoint().chosen_input.x) < 0.1f)
&& (fabsf(_selector.setpoint().chosen_input.y) < 0.1f);
const bool stick_lower_left = (_selector.setpoint().chosen_input.z < 0.1f)
&& (_selector.setpoint().chosen_input.r < -0.9f);
const bool stick_lower_right = (_selector.setpoint().chosen_input.z < 0.1f)
&& (_selector.setpoint().chosen_input.r > 0.9f);
_stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, _selector.setpoint().timestamp);
_stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, _selector.setpoint().timestamp);
@@ -153,14 +156,14 @@ void ManualControl::Run()
_selector.setpoint().user_override = rpy_moved || throttle_moved;
_x_diff.update(_selector.setpoint().x, dt_s);
_y_diff.update(_selector.setpoint().y, dt_s);
_z_diff.update(_selector.setpoint().z, dt_s);
_r_diff.update(_selector.setpoint().r, dt_s);
_x_diff.update(_selector.setpoint().chosen_input.x, dt_s);
_y_diff.update(_selector.setpoint().chosen_input.y, dt_s);
_z_diff.update(_selector.setpoint().chosen_input.z, dt_s);
_r_diff.update(_selector.setpoint().chosen_input.r, dt_s);
if (switches_updated) {
// Only use switches if current source is RC as well.
if (_selector.setpoint().data_source == manual_control_input_s::SOURCE_RC) {
if (_selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC) {
if (_previous_switches_initialized) {
if (switches.mode_slot != _previous_switches.mode_slot) {
evaluate_mode_slot(switches.mode_slot);
src/modules/manual_control/ManualControlSelector.cpp
+20 -20
View File
@@ -39,7 +39,7 @@ namespace manual_control
void ManualControlSelector::update_time_only(uint64_t now)
{
if (_setpoint.timestamp_sample + _timeout < now) {
if (_setpoint.chosen_input.timestamp_sample + _timeout < now) {
_setpoint.valid = false;
_instance = -1;
}
@@ -66,7 +66,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_5)) {
// We only stick to the first discovered mavlink channel.
if (_setpoint.data_source == input.data_source || !_setpoint.valid) {
if (_setpoint.chosen_input.data_source == input.data_source || !_setpoint.valid) {
_setpoint = setpoint_from_input(input);
_setpoint.valid = true;
_instance = instance;
@@ -77,7 +77,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu
} else if (_rc_in_mode == 3) {
// We only stick to the first discovered mavlink channel.
if (_setpoint.data_source == input.data_source || !_setpoint.valid) {
if (_setpoint.chosen_input.data_source == input.data_source || !_setpoint.valid) {
_setpoint = setpoint_from_input(input);
_setpoint.valid = true;
_instance = instance;
@@ -91,24 +91,24 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu
manual_control_setpoint_s ManualControlSelector::setpoint_from_input(const manual_control_input_s &input)
{
manual_control_setpoint_s setpoint;
setpoint.timestamp_sample = input.timestamp_sample;
setpoint.x = input.x;
setpoint.y = input.y;
setpoint.z = input.z;
setpoint.r = input.r;
setpoint.chosen_input.timestamp_sample = input.timestamp_sample;
setpoint.chosen_input.x = input.x;
setpoint.chosen_input.y = input.y;
setpoint.chosen_input.z = input.z;
setpoint.chosen_input.r = input.r;
// FIXME: what's that?
//setpoint.vx = (input.x - _manual_control_input[i].x) * dt_inv;
//setpoint.vy = (input.y - _manual_control_input[i].y) * dt_inv;
//setpoint.vz = (input.z - _manual_control_input[i].z) * dt_inv;
//setpoint.vr = (input.r - _manual_control_input[i].r) * dt_inv;
setpoint.flaps = input.flaps;
setpoint.aux1 = input.aux1;
setpoint.aux2 = input.aux2;
setpoint.aux3 = input.aux3;
setpoint.aux4 = input.aux4;
setpoint.aux5 = input.aux5;
setpoint.aux6 = input.aux6;
setpoint.data_source = input.data_source;
//setpoint.chosen_input.vx = (input.x - _manual_control_input[i].x) * dt_inv;
//setpoint.chosen_input.vy = (input.y - _manual_control_input[i].y) * dt_inv;
//setpoint.chosen_input.vz = (input.z - _manual_control_input[i].z) * dt_inv;
//setpoint.chosen_input.vr = (input.r - _manual_control_input[i].r) * dt_inv;
setpoint.chosen_input.flaps = input.flaps;
setpoint.chosen_input.aux1 = input.aux1;
setpoint.chosen_input.aux2 = input.aux2;
setpoint.chosen_input.aux3 = input.aux3;
setpoint.chosen_input.aux4 = input.aux4;
setpoint.chosen_input.aux5 = input.aux5;
setpoint.chosen_input.aux6 = input.aux6;
setpoint.chosen_input.data_source = input.data_source;
return setpoint;
}