From 01fd84e4dc0aaa1fbf82f7355cca241a76ece602 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 17:40:06 +0200 Subject: [PATCH] mavlink and commander app: Add support for manual input, either directly or via remote control --- msg/vehicle_status.msg | 4 ++ src/drivers/drv_rc_input.h | 3 +- src/modules/commander/commander.cpp | 4 +- src/modules/commander/commander_params.c | 10 +-- src/modules/mavlink/mavlink_main.cpp | 3 + src/modules/mavlink/mavlink_main.h | 18 +++++ src/modules/mavlink/mavlink_receiver.cpp | 66 ++++++++++++++----- .../navigator/mission_feasibility_checker.cpp | 6 +- 8 files changed, 87 insertions(+), 27 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 1e17b72c67..4d9716f6c9 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -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 diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index a8a76c3eff..fce5249b5e 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -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 }; /** diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5bbcf226a2..2e68de08d2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 2a4c91c554..2225b00d35 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -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); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index be63ae861c..9be21b25d7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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 */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b732355b44..71749e2923 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -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 */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8f31b2c858..8801a92aa2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -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); + } } } diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 8f1d6f8e85..d9297f25bf 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -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; }