fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 04:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink and commander app: Add support for manual input, either directly or via remote control

Browse Source
This commit is contained in:
Lorenz Meier
2015-05-24 17:40:06 +02:00
parent af546def91
commit 01fd84e4dc
8 changed files with 87 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+49 -17
View File
@@ -56,6 +56,7 @@
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_range_finder.h>
#include <drivers/drv_rc_input.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
@@ -914,29 +915,60 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
return;
}
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
if (_mavlink->get_manual_input_mode_generation()) {
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;
struct rc_input_values rc = {};
rc.timestamp_publication = hrt_absolute_time();
rc.timestamp_last_signal = rc.timestamp_publication;
manual.mode_switch = decode_switch_pos(man.buttons, 0);
manual.return_switch = decode_switch_pos(man.buttons, 1);
manual.posctl_switch = decode_switch_pos(man.buttons, 2);
manual.loiter_switch = decode_switch_pos(man.buttons, 3);
manual.acro_switch = decode_switch_pos(man.buttons, 4);
manual.offboard_switch = decode_switch_pos(man.buttons, 5);
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 = (2 * rc.channel_count + 4) * 1000;
rc.input_source = RC_INPUT_SOURCE_MAVLINK;
rc.values[0] = man.x * 500 + 1500;
rc.values[1] = man.y * 500 + 1500;
rc.values[2] = man.r / 2.0f + 1500;
rc.values[3] = man.z / 2.0f + 1500;
// warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
rc.values[4] = decode_switch_pos(man.buttons, 0) * 1000 + 1000;
rc.values[5] = decode_switch_pos(man.buttons, 1) * 1000 + 1000;
rc.values[6] = decode_switch_pos(man.buttons, 2) * 1000 + 1000;
rc.values[7] = decode_switch_pos(man.buttons, 3) * 1000 + 1000;
rc.values[8] = decode_switch_pos(man.buttons, 4) * 1000 + 1000;
rc.values[9] = decode_switch_pos(man.buttons, 5) * 1000 + 1000;
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
if (_rc_pub <= 0) {
_rc_pub = orb_advertise(ORB_ID(input_rc), &rc);
} else {
orb_publish(ORB_ID(input_rc), _rc_pub, &rc);
}
} else {
orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
struct 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.mode_switch = decode_switch_pos(man.buttons, 0);
manual.return_switch = decode_switch_pos(man.buttons, 1);
manual.posctl_switch = decode_switch_pos(man.buttons, 2);
manual.loiter_switch = decode_switch_pos(man.buttons, 3);
manual.acro_switch = decode_switch_pos(man.buttons, 4);
manual.offboard_switch = decode_switch_pos(man.buttons, 5);
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
} else {
orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
}
}
}