fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 14: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/mavlink/mavlink_main.cpp
-2
View File
@@ -2315,8 +2315,6 @@ Mavlink::task_main(int argc, char *argv[])
/* switch HIL mode if required */
set_hil_enabled(vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON);
set_generate_virtual_rc_input(vehicle_status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED);
if (_mode == MAVLINK_MODE_IRIDIUM) {
if (_transmitting_enabled && vehicle_status.high_latency_data_link_lost &&
src/modules/mavlink/mavlink_main.h
-18
View File
@@ -318,28 +318,11 @@ public:
*/
int set_hil_enabled(bool hil_enabled);
/**
* Set manual input generation mode
*
* Set to true to generate RC_INPUT messages on the system bus from
* MAVLink messages.
*
* @param generation_enabled If set to true, generate RC_INPUT messages
*/
void set_generate_virtual_rc_input(bool generation_enabled) { _generate_rc = generation_enabled; }
/**
* Set communication protocol for this mavlink instance
*/
void set_protocol(Protocol p) { _protocol = p; }
/**
* Get the manual input generation mode
*
* @return true if manual inputs should generate RC data
*/
bool should_generate_virtual_rc_input() { return _generate_rc; }
/**
* This is the beginning of a MAVLINK_START_UART_SEND/MAVLINK_END_UART_SEND transaction
*/
@@ -573,7 +556,6 @@ private:
/* states */
bool _hil_enabled{false}; /**< Hardware In the Loop mode */
bool _generate_rc{false}; /**< Generate RC messages from manual input MAVLink messages */
bool _is_usb_uart{false}; /**< Port is USB */
bool _wait_to_transmit{false}; /**< Wait to transmit until received messages. */
bool _received_messages{false}; /**< Whether we've received valid mavlink messages. */
src/modules/mavlink/mavlink_receiver.cpp
+10 -52
View File
@@ -2069,16 +2069,14 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
// fill uORB message
input_rc_s rc{};
// metadata
rc.timestamp = hrt_absolute_time();
rc.timestamp_last_signal = rc.timestamp;
rc.timestamp_last_signal = hrt_absolute_time();
rc.rssi = input_rc_s::RSSI_MAX;
rc.rc_failsafe = false;
rc.rc_lost = false;
rc.rc_lost_frame_count = 0;
rc.rc_total_frame_count = 1;
rc.rc_ppm_frame_length = 0;
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
// channels
@@ -2132,55 +2130,15 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
return;
}
if (_mavlink->should_generate_virtual_rc_input()) {
input_rc_s rc{};
rc.timestamp = hrt_absolute_time();
rc.timestamp_last_signal = rc.timestamp;
rc.channel_count = 8;
rc.rc_failsafe = false;
rc.rc_lost = false;
rc.rc_lost_frame_count = 0;
rc.rc_total_frame_count = 1;
rc.rc_ppm_frame_length = 0;
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
rc.rssi = input_rc_s::RSSI_MAX;
rc.values[0] = man.x / 2 + 1500; // roll
rc.values[1] = man.y / 2 + 1500; // pitch
rc.values[2] = man.r / 2 + 1500; // yaw
rc.values[3] = math::constrain(man.z / 0.9f + 800.0f, 1000.0f, 2000.0f); // throttle
/* decode all switches which fit into the channel mask */
unsigned max_switch = (sizeof(man.buttons) * 8);
unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0]));
if (max_switch > (max_channels - 4)) {
max_switch = (max_channels - 4);
}
/* fill all channels */
for (unsigned i = 0; i < max_switch; i++) {
rc.values[i + 4] = decode_switch_pos_n(man.buttons, i);
}
_mom_switch_state = man.buttons;
_rc_pub.publish(rc);
} else {
manual_control_setpoint_s manual{};
manual.timestamp = hrt_absolute_time();
manual.x = man.x / 1000.0f;
manual.y = man.y / 1000.0f;
manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f;
manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
_manual_control_setpoint_pub.publish(manual);
}
manual_control_input_s manual{};
manual.timestamp_sample = hrt_absolute_time();
manual.x = man.x / 1000.0f;
manual.y = man.y / 1000.0f;
manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f;
manual.data_source = manual_control_input_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
manual.timestamp = hrt_absolute_time();
_manual_control_input_pub.publish(manual);
}
void
src/modules/mavlink/mavlink_receiver.h
+2 -2
View File
@@ -80,7 +80,7 @@
#include <uORB/topics/irlock_report.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/log_message.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/onboard_computer_status.h>
@@ -331,7 +331,7 @@ private:
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
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::PublicationMulti<ping_s> _ping_pub{ORB_ID(ping)};
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status)};