From 3b2b270a40e0d8528339fe7cde5e0af91684fb97 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 21:55:12 +0100 Subject: mavlink: custom mode ACRO added --- src/modules/mavlink/mavlink.c | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d975066f..22465a3da 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -212,6 +212,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u if (v_status.main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_ACRO) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; } else if (v_status.main_state == MAIN_STATE_SEATBELT) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; -- cgit v1.2.3 From fb801b6faecd77fe2aac54d3389cacf73993ccc4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 20 May 2014 21:46:42 +0200 Subject: mavlink: minor fix --- src/modules/mavlink/mavlink_messages.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2837c16b8..933478f56 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -140,7 +140,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set } else if (status->main_state == MAIN_STATE_ACRO) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } } else { -- cgit v1.2.3 From c60561b705ddb557ce9b50cc3e41f36018708ef4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 May 2014 14:21:47 +0200 Subject: mavlink: Compile warning fixes --- src/modules/mavlink/mavlink_main.cpp | 15 +++++---------- src/modules/mavlink/mavlink_main.h | 4 +--- src/modules/mavlink/mavlink_orb_subscription.cpp | 2 +- src/modules/mavlink/mavlink_orb_subscription.h | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ src/modules/mavlink/mavlink_stream.cpp | 4 ++++ src/modules/mavlink/mavlink_stream.h | 6 +++++- 7 files changed, 19 insertions(+), 16 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6c97bfca7..28dd97fca 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -149,10 +149,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length instance = Mavlink::get_instance(6); break; #endif - } - - /* no valid instance, bail */ - if (!instance) { + default: return; } @@ -211,9 +208,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); Mavlink::Mavlink() : - next(nullptr), _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), + next(nullptr), _mavlink_fd(-1), _task_running(false), _hil_enabled(false), @@ -234,7 +231,6 @@ Mavlink::Mavlink() : _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), _message_buffer({}), - /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { @@ -2030,14 +2026,14 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, _subscribe_to_stream_rate); + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate); } else { warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); } } else { - warnx("stream %s not found", _subscribe_to_stream, _device_name); + warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); } delete _subscribe_to_stream; @@ -2243,7 +2239,6 @@ Mavlink::stream(int argc, char *argv[]) const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; const char *stream_name = nullptr; - int ch; argc -= 2; argv += 2; @@ -2280,7 +2275,7 @@ Mavlink::stream(int argc, char *argv[]) i++; } - if (!err_flag && rate >= 0.0 && stream_name != nullptr) { + if (!err_flag && rate >= 0.0f && stream_name != nullptr) { Mavlink *inst = get_instance_for_device(device_name); if (inst != nullptr) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c7a7d32f8..25c0da820 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -221,8 +221,6 @@ private: int _mavlink_fd; bool _task_running; - perf_counter_t _loop_perf; /**< loop performance counter */ - /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ @@ -282,7 +280,7 @@ private: pthread_mutex_t _message_buffer_mutex; - + perf_counter_t _loop_perf; /**< loop performance counter */ /** * Send one parameter. diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index d432edd2b..21d5219d3 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -63,7 +63,7 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription() free(_data); } -const orb_id_t +orb_id_t MavlinkOrbSubscription::get_topic() { return _topic; diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 5c6543e81..8c09772c8 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -63,7 +63,7 @@ public: */ bool is_published(); void *get_data(); - const orb_id_t get_topic(); + orb_id_t get_topic(); private: const orb_id_t _topic; /*< topic metadata */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 72b9ee83a..53769e0cf 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -932,6 +932,8 @@ void *MavlinkReceiver::start_helper(void *context) rcv->receive_thread(NULL); delete rcv; + + return nullptr; } pthread_t diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index bb19d7e33..5ec30bd33 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -81,5 +81,9 @@ MavlinkStream::update(const hrt_abstime t) /* interval expired, send message */ send(t); _last_sent = (t / _interval) * _interval; + + return 0; } + + return -1; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index def40d9ad..2979d20de 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -63,9 +63,13 @@ public: MavlinkStream *next; MavlinkStream(); - ~MavlinkStream(); + virtual ~MavlinkStream(); void set_interval(const unsigned int interval); void set_channel(mavlink_channel_t channel); + + /** + * @return 0 if updated / sent, -1 if unchanged + */ int update(const hrt_abstime t); virtual MavlinkStream *new_instance() = 0; virtual void subscribe(Mavlink *mavlink) = 0; -- cgit v1.2.3 From 00d40d095ddb4a5ad645044bed1ffdc1f977dba3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 May 2014 00:12:59 +0200 Subject: mavlink mission item takeoff: read correct param for minimal pitch --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 28dd97fca..bb1ad86ef 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -415,7 +415,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - + Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { @@ -886,7 +886,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item switch (mavlink_mission_item->command) { case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param2; + mission_item->pitch_min = mavlink_mission_item->param1; break; default: -- cgit v1.2.3 From 46301f753d212b55e151a394bc9d4b3b787f35ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 18:28:37 +0200 Subject: Minor fixes to MAVLink --- src/modules/mavlink/mavlink_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bb1ad86ef..28cdf65a3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -190,8 +190,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length /* check if there is space in the buffer, let it overflow else */ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - if (desired > buf_free) { - desired = buf_free; + if (buf_free < desired) { + /* we don't want to send anything just in half, so return */ + return; } } @@ -222,6 +223,8 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _mode(MAVLINK_MODE_NORMAL), + _total_counter(0), _verbose(false), _forwarding_on(false), _passing_on(false), -- cgit v1.2.3 From 138edca5445a66df3647c03529ad2a30a51fe083 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 31 May 2014 13:15:10 +0200 Subject: attempt to fix mavlink hardware flow control disable logic --- src/modules/mavlink/mavlink_main.cpp | 32 +++++++++++++++++--------------- 1 file changed, 17 insertions(+), 15 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 28cdf65a3..540b88657 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -105,7 +105,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); -static uint64_t last_write_times[6] = {0}; +static uint64_t last_write_success_times[6] = {0}; +static uint64_t last_write_try_times[6] = {0}; /* * Internal function to send the bytes through the right serial port @@ -166,26 +167,25 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (instance->get_flow_control_enabled() && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - if (buf_free == 0) { - - if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { - - warnx("DISABLING HARDWARE FLOW CONTROL"); - instance->enable_flow_control(false); - } - - } else { - - /* apparently there is space left, although we might be - * partially overflooding the buffer already */ - last_write_times[(unsigned)channel] = hrt_absolute_time(); + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (last_write_try_times[(unsigned)channel] != 0 && + hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && + last_write_success_times[(unsigned)channel] != + last_write_try_times[(unsigned)channel]) + { + warnx("DISABLING HARDWARE FLOW CONTROL"); + instance->enable_flow_control(false); } + } /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (instance->should_transmit()) { + last_write_try_times[(unsigned)channel] = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { @@ -199,6 +199,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length ssize_t ret = write(uart, ch, desired); if (ret != desired) { warnx("TX FAIL"); + } else { + last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } } -- cgit v1.2.3 From 794b853a3b94b5520b734933f18823885259427d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 14:04:16 +0200 Subject: mavlink: for takeoff mission items don't set the time inside field. Correctly set pitch min when converting from mission item to mavlink mission item --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 540b88657..304a02c29 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -896,6 +896,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item default: mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->time_inside = mavlink_mission_item->param1; break; } @@ -904,7 +905,6 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; - mission_item->time_inside = mavlink_mission_item->param1; mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; @@ -923,11 +923,12 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ switch (mission_item->nav_cmd) { case NAV_CMD_TAKEOFF: - mavlink_mission_item->param2 = mission_item->pitch_min; + mavlink_mission_item->param1 = mission_item->pitch_min; break; default: mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param1 = mission_item->time_inside; break; } @@ -938,7 +939,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->param1 = mission_item->time_inside; mavlink_mission_item->autocontinue = mission_item->autocontinue; // mavlink_mission_item->seq = mission_item->index; -- cgit v1.2.3 From 325d5026bb6d36ab3557ab999f8db772f77ac7a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Jun 2014 13:37:31 +0200 Subject: Hotfix: Fix scaling for battery current --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 933478f56..fd41b723a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -274,7 +274,7 @@ protected: status->onboard_control_sensors_health, status->load * 1000.0f, status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, + status->battery_current * 100.0f, status->battery_remaining * 100.0f, status->drop_rate_comm, status->errors_comm, -- cgit v1.2.3 From 5624c1406aa78aa4bf4b5c0e20dca637c26478d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Jun 2014 16:43:15 +0200 Subject: Hotfix: Better microSD reporting --- src/modules/mavlink/mavlink_main.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 304a02c29..264cfd875 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1054,6 +1054,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t } else { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); if (_verbose) { warnx("ERROR: could not read WP%u", seq); } } @@ -1440,6 +1441,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } -- cgit v1.2.3 From 33067373614e50e3be068d30f3ad3b718d16df5f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 6 Jun 2014 00:42:02 +0200 Subject: mavlink: send MISSION_REQUEST after short timeout when receiving mission, remove all "target id mismatch" warnings --- src/modules/mavlink/mavlink_main.cpp | 51 ++++++++++-------------------------- src/modules/mavlink/mavlink_main.h | 1 + 2 files changed, 15 insertions(+), 37 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 264cfd875..e300be074 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -790,9 +790,14 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + mavlink_param_request_list_t req; + mavlink_msg_param_request_list_decode(msg, &req); + if (req.target_system == mavlink_system.sysid && + (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { + /* Start sending parameters */ + mavlink_pm_start_queued_send(); + mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + } } break; case MAVLINK_MSG_ID_PARAM_SET: { @@ -954,6 +959,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->current_partner_compid = 0; state->timestamp_lastaction = 0; state->timestamp_last_send_setpoint = 0; + state->timestamp_last_send_request = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->current_dataman_id = 0; } @@ -1070,6 +1076,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u wpr.seq = seq; mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr); mavlink_missionlib_send_message(&msg); + _wpm->timestamp_last_send_request = hrt_absolute_time(); if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } @@ -1112,6 +1119,10 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) _wpm->current_state = MAVLINK_WPM_STATE_IDLE; _wpm->current_partner_sysid = 0; _wpm->current_partner_compid = 0; + + } else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + /* try to get WP again after short timeout */ + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } @@ -1174,11 +1185,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } } break; @@ -1211,11 +1217,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } } - - } else { - mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - - if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } } break; @@ -1304,12 +1305,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } @@ -1368,12 +1363,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } break; @@ -1473,11 +1462,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1513,13 +1497,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } } - - - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 25c0da820..40edc4b85 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -93,6 +93,7 @@ struct mavlink_wpm_storage { uint8_t current_partner_compid; uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_last_send_request; uint32_t timeout; int current_dataman_id; }; -- cgit v1.2.3