fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 22:14:08 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/vehicle_status.msg
View File

@ -81,6 +81,10 @@ uint8 VEHICLE_BATTERY_WARNING_NONE = 0 # no battery low voltage warning acti
uint8 VEHICLE_BATTERY_WARNING_LOW = 1 # warning of low voltage
uint8 VEHICLE_BATTERY_WARNING_CRITICAL = 2 # alerting of critical voltage
uint8 RC_IN_MODE_DEFAULT = 0
uint8 RC_IN_MODE_OFF = 1
uint8 RC_IN_MODE_GENERATED = 2
# state machine / state of vehicle.
# Encodes the complete system state and is set by the commander app.
uint16 counter # incremented by the writing thread everytime new data is stored

3
src/drivers/drv_rc_input.h
View File

@ -99,7 +99,8 @@ enum RC_INPUT_SOURCE {
RC_INPUT_SOURCE_PX4IO_PPM,
RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
RC_INPUT_SOURCE_PX4IO_SBUS,
RC_INPUT_SOURCE_PX4IO_ST24
RC_INPUT_SOURCE_PX4IO_ST24,
RC_INPUT_SOURCE_MAVLINK
};
/**

4
src/modules/commander/commander.cpp
View File

@ -828,7 +828,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_ef_time_thres = param_find("COM_EF_TIME");
param_t _param_autostart_id = param_find("SYS_AUTOSTART");
param_t _param_autosave_params = param_find("COM_AUTOS_PAR");
param_t _param_rc_in_off = param_find("COM_RC_IN_OFF");
param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL";
@ -1237,7 +1237,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
param_get(_param_rc_loss_timeout, &rc_loss_timeout);
param_get(_param_rc_in_off, &rc_in_off);
status.rc_input_off = rc_in_off;
status.rc_input_off = (rc_in_off == vehicle_status_s::RC_IN_MODE_OFF);
param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);

10
src/modules/commander/commander_params.c
View File

@ -199,11 +199,13 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1);
/**
* Disable RC control input
*
* Setting this to 1 disables RC input handling and the associated checks. This is
* mainly inteded for pure offboard control or tablet control use.
* The default value of 0 requires a valid RC transmitter setup.
* Setting this to 1 disables RC input handling and the associated checks. A value of
* 2 will generate RC control data from manual input received via MAVLink instead
* of directly forwarding the manual input data.
*
* @group Commander
* @min 0
* @max 1
* @max 2
*/
PARAM_DEFINE_INT32(COM_RC_IN_OFF, 0);
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);

3
src/modules/mavlink/mavlink_main.cpp
View File

@ -123,6 +123,7 @@ Mavlink::Mavlink() :
_mavlink_fd(-1),
_task_running(false),
_hil_enabled(false),
_generate_rc(false),
_use_hil_gps(false),
_forward_externalsp(false),
_is_usb_uart(false),
@ -1497,6 +1498,8 @@ Mavlink::task_main(int argc, char *argv[])
if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
set_manual_input_mode_generation(status.rc_input_off == vehicle_status_s::RC_IN_MODE_GENERATED);
}
/* check for requested subscriptions */

18
src/modules/mavlink/mavlink_main.h
View File

@ -170,6 +170,23 @@ 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_manual_input_mode_generation(bool generation_enabled) { _generate_rc = generation_enabled; }
/**
* Get the manual input generation mode
*
* @return true if manual inputs should generate RC data
*/
bool get_manual_input_mode_generation() { return _generate_rc; }
void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0);
/**
@ -287,6 +304,7 @@ private:
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _generate_rc; /**< Generate RC messages from manual input MAVLink messages */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */
bool _is_usb_uart; /**< Port is USB */

66
src/modules/mavlink/mavlink_receiver.cpp
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);
}
}
}

6
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -127,7 +127,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
}
if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
mavlink_log_critical(_mavlink_fd, "Geofence violation waypoint %d", i);
mavlink_log_critical(_mavlink_fd, "Geofence violation for waypoint %d", i);
return false;
}
}
@ -177,7 +177,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
if (dm_read(dm_current, i, &missionitem, len) != len) {
// not supposed to happen unless the datamanager can't access the SD card, etc.
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access mission from SD card");
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access SD card");
return false;
}
@ -197,7 +197,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
return false;
}
}
mavlink_log_critical(_mavlink_fd, "Mission is valid!");
mavlink_log_info(_mavlink_fd, "Mission ready.");
return true;
}