diff options
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rcS | 10 | ||||
-rw-r--r-- | src/modules/commander/commander.cpp | 35 | ||||
-rw-r--r-- | src/modules/dataman/dataman.c | 54 | ||||
-rw-r--r-- | src/modules/dataman/dataman.h | 16 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 749 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 95 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_mission.cpp | 818 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_mission.h | 176 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 13 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 2 | ||||
-rw-r--r-- | src/modules/mavlink/module.mk | 1 | ||||
-rw-r--r-- | src/modules/mavlink/util.h | 53 | ||||
-rw-r--r-- | src/modules/mavlink/waypoints.h | 111 | ||||
-rw-r--r-- | src/modules/navigator/mission.cpp | 124 | ||||
-rw-r--r-- | src/modules/navigator/mission.h | 7 | ||||
-rw-r--r-- | src/modules/uORB/topics/mission.h | 10 | ||||
-rw-r--r-- | src/modules/uORB/topics/mission_result.h | 8 |
17 files changed, 1277 insertions, 1005 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1c9ded6a8..8855539fe 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -280,6 +280,11 @@ then nshterm /dev/ttyACM0 & # + # Start the datamanager + # + dataman start + + # # Start the Commander (needs to be this early for in-air-restarts) # commander start @@ -548,11 +553,6 @@ then fi # - # Start the datamanager - # - dataman start - - # # Start the navigator # navigator start diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..d44df3086 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -78,6 +78,7 @@ #include <uORB/topics/differential_pressure.h> #include <uORB/topics/safety.h> #include <uORB/topics/system_power.h> +#include <uORB/topics/mission.h> #include <uORB/topics/mission_result.h> #include <uORB/topics/telemetry_status.h> @@ -92,6 +93,7 @@ #include <systemlib/cpuload.h> #include <systemlib/rc_check.h> #include <systemlib/state_table.h> +#include <dataman/dataman.h> #include "px4_custom_mode.h" #include "commander_helper.h" @@ -717,6 +719,11 @@ int commander_thread_main(int argc, char *argv[]) /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + if (status_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } /* armed topic */ orb_advert_t armed_pub; @@ -734,10 +741,27 @@ int commander_thread_main(int argc, char *argv[]) struct home_position_s home; memset(&home, 0, sizeof(home)); - if (status_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); - warnx("exiting."); - exit(ERROR); + /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ + orb_advert_t mission_pub = -1; + mission_s mission; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { + warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); + mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", + mission.dataman_id, mission.count, mission.current_seq); + } else { + warnx("reading mission state failed"); + mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + + /* initialize mission state in dataman */ + mission.dataman_id = 0; + mission.count = 0; + mission.current_seq = 0; + dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + } + + mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); } mavlink_log_info(mavlink_fd, "[cmd] started"); @@ -1437,7 +1461,7 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.mission_finished); + mission_result.finished); // TODO handle mode changes by commands if (main_state_changed) { @@ -1541,6 +1565,7 @@ int commander_thread_main(int argc, char *argv[]) close(diff_pres_sub); close(param_changed_sub); close(battery_sub); + close(mission_pub); thread_running = false; diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 406293bda..ca1fe9bbb 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -62,6 +62,8 @@ __EXPORT int dataman_main(int argc, char *argv[]); __EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen); __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen); __EXPORT int dm_clear(dm_item_t item); +__EXPORT void dm_lock(dm_item_t item); +__EXPORT void dm_unlock(dm_item_t item); __EXPORT int dm_restart(dm_reset_reason restart_type); /** Types of function calls supported by the worker task */ @@ -114,12 +116,17 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_FENCE_POINTS_MAX, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, - DM_KEY_WAYPOINTS_ONBOARD_MAX + DM_KEY_WAYPOINTS_ONBOARD_MAX, + DM_KEY_MISSION_STATE_MAX }; /* Table of offset for index 0 of each item type */ static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; +/* Item type lock mutexes */ +static sem_t *g_item_locks[DM_KEY_NUM_KEYS]; +static sem_t g_sys_state_mutex; + /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; static const char *k_data_manager_device_path = "/fs/microsd/dataman"; @@ -139,22 +146,22 @@ static work_q_t g_work_q; /* pending work items. To be consumed by worker thread sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ sem_t g_init_sema; -static bool g_task_should_exit; /**< if true, dataman task should exit */ +static bool g_task_should_exit; /**< if true, dataman task should exit */ #define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */ static void init_q(work_q_t *q) { - sq_init(&(q->q)); /* Initialize the NuttX queue structure */ + sq_init(&(q->q)); /* Initialize the NuttX queue structure */ sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ - q->size = q->max_size = 0; /* Queue is initially empty */ + q->size = q->max_size = 0; /* Queue is initially empty */ } static inline void destroy_q(work_q_t *q) { - sem_destroy(&(q->mutex)); /* Destroy the queue lock */ + sem_destroy(&(q->mutex)); /* Destroy the queue lock */ } static inline void @@ -318,7 +325,9 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v buffer[1] = persistence; buffer[2] = 0; buffer[3] = 0; - memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + if (count > 0) { + memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + } count += DM_SECTOR_HDR_SIZE; len = -1; @@ -568,6 +577,32 @@ dm_clear(dm_item_t item) return enqueue_work_item_and_wait_for_result(work); } +__EXPORT void +dm_lock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if ((g_fd < 0) || g_task_should_exit) + return; + if (item >= DM_KEY_NUM_KEYS) + return; + if (g_item_locks[item]) { + sem_wait(g_item_locks[item]); + } +} + +__EXPORT void +dm_unlock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if ((g_fd < 0) || g_task_should_exit) + return; + if (item >= DM_KEY_NUM_KEYS) + return; + if (g_item_locks[item]) { + sem_post(g_item_locks[item]); + } +} + /* Tell the data manager about the type of the last reset */ __EXPORT int dm_restart(dm_reset_reason reason) @@ -608,6 +643,12 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < dm_number_of_funcs; i++) g_func_counts[i] = 0; + /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */ + sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ + for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) + g_item_locks[i] = NULL; + g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex; + g_task_should_exit = false; init_q(&g_work_q); @@ -743,6 +784,7 @@ task_main(int argc, char *argv[]) destroy_q(&g_work_q); destroy_q(&g_free_q); sem_destroy(&g_work_queued_sema); + sem_destroy(&g_sys_state_mutex); return 0; } diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 1dfb26f73..dbace21ef 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,6 +53,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -62,7 +63,8 @@ extern "C" { DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_MISSION_STATE_MAX = 1 }; /** Data persistence levels */ @@ -101,6 +103,18 @@ extern "C" { size_t buflen /* Length in bytes of data to retrieve */ ); + /** Lock all items of this type */ + __EXPORT void + dm_lock( + dm_item_t item /* The item type to clear */ + ); + + /** Unlock all items of this type */ + __EXPORT void + dm_unlock( + dm_item_t item /* The item type to clear */ + ); + /** Erase all items of this type */ __EXPORT int dm_clear( diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 026a4d6c9..5c7eefa98 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -18,7 +18,6 @@ * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, @@ -73,8 +72,6 @@ #include <mavlink/mavlink_log.h> #include <uORB/topics/parameter_update.h> -#include <uORB/topics/mission.h> -#include <uORB/topics/mission_result.h> #include "mavlink_bridge_header.h" #include "mavlink_main.h" @@ -109,6 +106,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +extern mavlink_system_t mavlink_system; + static uint64_t last_write_success_times[6] = {0}; static uint64_t last_write_try_times[6] = {0}; @@ -209,9 +208,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } } - - - } static void usage(void); @@ -230,6 +226,7 @@ Mavlink::Mavlink() : _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), + _mission_result_sub(-1), _mission_pub(-1), _mode(MAVLINK_MODE_NORMAL), _total_counter(0), @@ -248,13 +245,12 @@ Mavlink::Mavlink() : _param_component_id(0), _param_system_type(0), _param_use_hil_gps(0), + _mission_manager(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { - _wpm = &_wpm_s; - mission.count = 0; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); @@ -439,7 +435,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) } void -Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) { Mavlink *inst; @@ -515,7 +511,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) void Mavlink::mavlink_update_system(void) { - if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); @@ -729,9 +724,32 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -extern mavlink_system_t mavlink_system; +void +Mavlink::send_message(const mavlink_message_t *msg) +{ + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + mavlink_send_uart_bytes(_channel, buf, len); +} + +void +Mavlink::handle_message(const mavlink_message_t *msg) +{ + /* handle packet with mission manager */ + _mission_manager->handle_message(msg); -int Mavlink::mavlink_pm_queued_send() + /* handle packet with parameter component */ + mavlink_pm_message_handler(_channel, msg); + + if (get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(msg, this); + } +} + +int +Mavlink::mavlink_pm_queued_send() { if (_mavlink_param_queue_index < param_count()) { mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); @@ -808,7 +826,7 @@ int Mavlink::mavlink_pm_send_param(param_t param) mavlink_type, param_count(), param_get_index(param)); - mavlink_missionlib_send_message(&tx_msg); + send_message(&tx_msg); return OK; } @@ -822,7 +840,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav (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"); + send_statustext("[mavlink pm] sending list"); } } break; @@ -846,7 +864,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown: %s", name); - mavlink_missionlib_send_gcs_string(buf); + send_statustext(buf); } else { /* set and send parameter */ @@ -882,653 +900,18 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav } } -void Mavlink::publish_mission() -{ - /* Initialize mission publication if necessary */ - if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); - - } else { - orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); - } -} - -int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) -{ - /* only support global waypoints for now */ - switch (mavlink_mission_item->frame) { - case MAV_FRAME_GLOBAL: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = false; - break; - - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = true; - break; - - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; - - case MAV_FRAME_MISSION: - default: - return MAV_MISSION_ERROR; - } - - switch (mavlink_mission_item->command) { - case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param1; - break; - case MAV_CMD_DO_JUMP: - mission_item->do_jump_mission_index = mavlink_mission_item->param1; - mission_item->do_jump_current_count = 0; - mission_item->do_jump_repeat_count = mavlink_mission_item->param2; - break; - default: - mission_item->acceptance_radius = mavlink_mission_item->param2; - mission_item->time_inside = mavlink_mission_item->param1; - break; - } - - mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); - mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); - 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->autocontinue = mavlink_mission_item->autocontinue; - // mission_item->index = mavlink_mission_item->seq; - mission_item->origin = ORIGIN_MAVLINK; - - /* reset DO_JUMP count */ - mission_item->do_jump_current_count = 0; - - return OK; -} - -int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) -{ - if (mission_item->altitude_is_relative) { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL; - - } else { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - } - - switch (mission_item->nav_cmd) { - case NAV_CMD_TAKEOFF: - mavlink_mission_item->param1 = mission_item->pitch_min; - break; - - case NAV_CMD_DO_JUMP: - mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; - break; - - default: - mavlink_mission_item->param2 = mission_item->acceptance_radius; - mavlink_mission_item->param1 = mission_item->time_inside; - break; - } - - mavlink_mission_item->x = (float)mission_item->lat; - mavlink_mission_item->y = (float)mission_item->lon; - mavlink_mission_item->z = mission_item->altitude; - - 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->autocontinue = mission_item->autocontinue; - // mavlink_mission_item->seq = mission_item->index; - - return OK; -} - -void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) -{ - state->size = 0; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - 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; -} - -/* - * @brief Sends an waypoint ack message - */ -void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = sysid; - wpa.target_component = compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if (seq < _wpm->size) { - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = seq; - - mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - } else if (seq == 0 && _wpm->size == 0) { - - /* don't broadcast if no WPs */ - - } else { - mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - } -} - -void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = sysid; - wpc.target_component = compid; - wpc.count = mission.count; - - mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } -} - -void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - - struct mission_item_s mission_item; - ssize_t len = sizeof(struct mission_item_s); - - dm_item_t dm_current; - - if (_wpm->current_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - if (dm_read(dm_current, seq, &mission_item, len) == len) { - - /* create mission_item_s from mavlink_mission_item_t */ - mavlink_mission_item_t wp; - map_mission_item_to_mavlink_mission_item(&mission_item, &wp); - - mavlink_message_t msg; - wp.target_system = sysid; - wp.target_component = compid; - wp.seq = seq; - mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } - - } 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); } - } -} - -void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < _wpm->max_size) { - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = sysid; - wpr.target_component = compid; - 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); } - - } else { - mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } -} - -void Mavlink::mavlink_waypoint_eventloop(uint64_t now) -{ - /* check for timed-out operations */ - if (now - _wpm->timestamp_lastaction > _wpm->timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("Operation timeout"); - - _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); - } -} - - -void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) -{ - uint64_t now = hrt_absolute_time(); - - switch (msg->msgid) { - - case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (_wpm->current_wp_id == _wpm->size - 1) { - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_wp_id = 0; - } - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { - mavlink_mission_set_current_t wpc; - mavlink_msg_mission_set_current_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < _wpm->size) { - - mission.current_index = wpc.seq; - publish_mission(); - - /* don't answer yet, wait for the navigator to respond, then publish the mission_result */ -// mavlink_wpm_send_waypoint_current(wpc.seq); - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - } - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - mavlink_mission_request_list_t wprl; - mavlink_msg_mission_request_list_decode(msg, &wprl); - - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (_wpm->size > 0) { - - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - - } else { - if (_verbose) { warnx("No waypoints send"); } - } - - _wpm->current_count = _wpm->size; - mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count); - - } else { - mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - - if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (wpr.seq >= _wpm->size) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - - break; - } - - /* - * Ensure that we are in the correct state and that the first request has id 0 - * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - - if (wpr.seq == 0) { - if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - - if (wpr.seq == _wpm->current_wp_id) { - - if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else if (wpr.seq == _wpm->current_wp_id + 1) { - - if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - - break; - } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - - break; - } - - _wpm->current_wp_id = wpr.seq; - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - if (wpr.seq < _wpm->size) { - - mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - - mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds"); - } - - - } else { - //we we're target but already communicating with someone else - if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { - - 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); } - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - - if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); - break; - } - - if (wpc.count == 0) { - mavlink_missionlib_send_gcs_string("WP COUNT 0"); - - break; - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - _wpm->current_count = wpc.count; - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - - if (_wpm->current_wp_id == 0) { - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP"); - } - - } else { - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - } - } - } - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - - _wpm->timestamp_lastaction = now; - - /* - * ensure that we are in the correct state and that the first waypoint has id 0 - * and the following waypoints have the correct ids - */ - - if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - - if (wp.seq != 0) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - - if (wp.seq >= _wpm->current_count) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - break; - } - - if (wp.seq != _wpm->current_wp_id) { - mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch"); - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - break; - } - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - - struct mission_item_s mission_item; - - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); - - if (ret != OK) { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - ssize_t len = sizeof(struct mission_item_s); - - dm_item_t dm_next; - - if (_wpm->current_dataman_id == 0) { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; - mission.dataman_id = 1; - - } else { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.dataman_id = 0; - } - - 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; - } - -// if (wp.current) { -// warnx("current is: %d", wp.seq); -// mission.current_index = wp.seq; -// } - // XXX ignore current set - mission.current_index = -1; - - _wpm->current_wp_id = wp.seq + 1; - - if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - - if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - mission.count = _wpm->current_count; - - publish_mission(); - - _wpm->current_dataman_id = mission.dataman_id; - _wpm->size = _wpm->current_count; - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - - } else { - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - _wpm->timestamp_lastaction = now; - - _wpm->size = 0; - - /* prepare mission topic */ - mission.dataman_id = -1; - mission.count = 0; - mission.current_index = -1; - publish_mission(); - - if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } - - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - - if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } - } - } - - break; - } - - default: { - /* other messages might should get caught by mavlink and others */ - break; - } - } -} - -void -Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) +int +Mavlink::send_statustext(const char *string) { - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - mavlink_send_uart_bytes(_channel, buf, len); + return send_statustext(MAV_SEVERITY_INFO, string); } - - int -Mavlink::mavlink_missionlib_send_gcs_string(const char *string) +Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; - statustext.severity = MAV_SEVERITY_INFO; + statustext.severity = severity; int i = 0; @@ -1550,7 +933,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) return OK; } else { - return 1; + return ERROR; } } @@ -1702,7 +1085,7 @@ Mavlink::message_buffer_is_empty() bool -Mavlink::message_buffer_write(void *ptr, int size) +Mavlink::message_buffer_write(const void *ptr, int size) { // bytes available to write int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; @@ -1769,7 +1152,7 @@ Mavlink::message_buffer_mark_read(int n) } void -Mavlink::pass_message(mavlink_message_t *msg) +Mavlink::pass_message(const mavlink_message_t *msg) { if (_passing_on) { /* size is 8 bytes plus variable payload */ @@ -1780,7 +1163,11 @@ Mavlink::pass_message(mavlink_message_t *msg) } } - +float +Mavlink::get_rate_mult() +{ + return _datarate / 1000.0f; +} int Mavlink::task_main(int argc, char *argv[]) @@ -1902,8 +1289,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1948,12 +1333,11 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - /* initialize waypoint manager */ - mavlink_wpm_init(_wpm); + _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - struct mission_result_s mission_result; - memset(&mission_result, 0, sizeof(mission_result)); + /* create mission manager */ + _mission_manager = new MavlinkMissionManager(this); + _mission_manager->set_verbose(_verbose); _task_running = true; @@ -1968,7 +1352,7 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkCommandsStream commands_stream(this, _channel); /* add default streams depending on mode and intervals depending on datarate */ - float rate_mult = _datarate / 1000.0f; + float rate_mult = get_rate_mult(); configure_stream("HEARTBEAT", 1.0f); @@ -2003,7 +1387,6 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ @@ -2057,36 +1440,16 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - bool updated; - orb_check(mission_result_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); - - if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); } - - if (mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); - } - - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); - - } else { - if (slow_rate_limiter.check(t)) { - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); - } - } - if (fast_rate_limiter.check(t)) { mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(hrt_absolute_time()); + _mission_manager->eventloop(); if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { - mavlink_missionlib_send_gcs_string(msg.text); + send_statustext(msg.text); } } } @@ -2138,11 +1501,11 @@ Mavlink::task_main(int argc, char *argv[]) } } - - perf_end(_loop_perf); } + delete _mission_manager; + delete _subscribe_to_stream; _subscribe_to_stream = nullptr; @@ -2238,7 +1601,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1950, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index f94036a17..1f4fb759f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -50,53 +50,13 @@ #include <uORB/uORB.h> #include <uORB/topics/mission.h> +#include <uORB/topics/mission_result.h> #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" #include "mavlink_stream.h" #include "mavlink_messages.h" - -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; - uint16_t max_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_count; - uint8_t current_partner_sysid; - 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; -}; +#include "mavlink_mission.h" class Mavlink @@ -139,7 +99,7 @@ public: static bool instance_exists(const char *device_name, Mavlink *self); - static void forward_message(mavlink_message_t *msg, Mavlink *self); + static void forward_message(const mavlink_message_t *msg, Mavlink *self); static int get_uart_fd(unsigned index); @@ -178,11 +138,6 @@ public: bool get_forwarding_on() { return _forwarding_on; } - /** - * Handle waypoint related messages. - */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); - static int start_helper(int argc, char *argv[]); /** @@ -202,7 +157,11 @@ public: */ int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + void send_message(const mavlink_message_t *msg); + + void handle_message(const mavlink_message_t *msg); + + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); int get_instance_id(); @@ -221,8 +180,11 @@ public: int get_mavlink_fd() { return _mavlink_fd; } + int send_statustext(const char *string); + int send_statustext(enum MAV_SEVERITY severity, const char *string); MavlinkStream * get_streams() const { return _streams; } + float get_rate_mult(); /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } @@ -231,7 +193,7 @@ public: bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } - bool message_buffer_write(void *ptr, int size); + bool message_buffer_write(const void *ptr, int size); void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } @@ -262,22 +224,19 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - orb_advert_t _mission_pub; - struct mission_s mission; - MAVLINK_MODE _mode; + MavlinkMissionManager *_mission_manager; - uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _channel; + orb_advert_t _mission_pub; + int _mission_result_sub; + MAVLINK_MODE _mode; + + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; pthread_t _receive_thread; - /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; - bool _verbose; bool _forwarding_on; bool _passing_on; @@ -363,21 +322,6 @@ private: void mavlink_update_system(); - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); - void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); - void mavlink_wpm_send_waypoint_current(uint16_t seq); - void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); - void mavlink_wpm_init(mavlink_wpm_storage *state); - int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); - void publish_mission(); - - void mavlink_missionlib_send_message(mavlink_message_t *msg); - int mavlink_missionlib_send_gcs_string(const char *string); - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); @@ -394,7 +338,7 @@ private: void message_buffer_mark_read(int n); - void pass_message(mavlink_message_t *msg); + void pass_message(const mavlink_message_t *msg); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); @@ -402,5 +346,4 @@ private: * Main mavlink task. */ int task_main(int argc, char *argv[]); - }; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp new file mode 100644 index 000000000..b2288469c --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -0,0 +1,818 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_mission.cpp + * MAVLink mission manager implementation. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include "mavlink_mission.h" +#include "mavlink_main.h" + +#include <math.h> +#include <lib/geo/geo.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> + +#include <dataman/dataman.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/mission_result.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + + +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : + _mavlink(mavlink), + _channel(mavlink->get_channel()), + _comp_id(MAV_COMP_ID_MISSIONPLANNER), + _state(MAVLINK_WPM_STATE_IDLE), + _time_last_recv(0), + _time_last_sent(0), + _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), + _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), + _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), + _count(0), + _current_seq(0), + _dataman_id(0), + _transfer_dataman_id(0), + _transfer_count(0), + _transfer_seq(0), + _transfer_current_seq(0), + _transfer_partner_sysid(0), + _transfer_partner_compid(0), + _offboard_mission_sub(-1), + _mission_result_sub(-1), + _offboard_mission_pub(-1), + _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), + _verbose(false) +{ + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + + init_offboard_mission(); +} + +MavlinkMissionManager::~MavlinkMissionManager() +{ + close(_offboard_mission_pub); + close(_mission_result_sub); +} + +void +MavlinkMissionManager::init_offboard_mission() +{ + mission_s mission_state; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) { + _dataman_id = mission_state.dataman_id; + _count = mission_state.count; + _current_seq = mission_state.current_seq; + + warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq); + + } else { + _dataman_id = 0; + _count = 0; + _current_seq = 0; + warnx("offboard mission init: ERROR, reading mission state failed"); + } +} + +/** + * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes. + */ +int +MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq) +{ + struct mission_s mission; + + mission.dataman_id = dataman_id; + mission.count = count; + mission.current_seq = seq; + + /* update mission state in dataman */ + int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + + if (res == sizeof(mission_s)) { + /* update active mission state */ + _dataman_id = dataman_id; + _count = count; + _current_seq = seq; + + /* mission state saved successfully, publish offboard_mission topic */ + if (_offboard_mission_pub < 0) { + _offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + + } else { + orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission); + } + + return OK; + + } else { + warnx("ERROR: can't save mission state"); + _mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state"); + + return ERROR; + } +} + +void +MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_mission_ack_t wpa; + + wpa.target_system = sysid; + wpa.target_component = compid; + wpa.type = type; + + mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } +} + + +void +MavlinkMissionManager::send_mission_current(uint16_t seq) +{ + if (seq < _count) { + mavlink_message_t msg; + mavlink_mission_current_t wpc; + + wpc.seq = seq; + + mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); + _mavlink->send_message(&msg); + + } else if (seq == 0 && _count == 0) { + /* don't broadcast if no WPs */ + + } else { + if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); } + + _mavlink->send_statustext("ERROR: wp index out of bounds"); + } +} + + +void +MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + _time_last_sent = hrt_absolute_time(); + + mavlink_message_t msg; + mavlink_mission_count_t wpc; + + wpc.target_system = sysid; + wpc.target_component = compid; + wpc.count = _count; + + mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } +} + + +void +MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + dm_item_t dm_item = _dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + struct mission_item_s mission_item; + + if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + _time_last_sent = hrt_absolute_time(); + + /* create mission_item_s from mavlink_mission_item_t */ + mavlink_mission_item_t wp; + format_mavlink_mission_item(&mission_item, &wp); + + mavlink_message_t msg; + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + wp.current = (_current_seq == seq) ? 1 : 0; + mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("#audio: Unable to read from micro SD"); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } + } +} + + +void +MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < _max_count) { + _time_last_sent = hrt_absolute_time(); + + mavlink_message_t msg; + mavlink_mission_request_t wpr; + wpr.target_system = sysid; + wpr.target_component = compid; + wpr.seq = seq; + mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } + + } else { + _mavlink->send_statustext("ERROR: Waypoint index exceeds list capacity"); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); } + } +} + + +void +MavlinkMissionManager::send_mission_item_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_mission_item_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } +} + + +void +MavlinkMissionManager::eventloop() +{ + hrt_abstime now = hrt_absolute_time(); + + bool updated = false; + orb_check(_mission_result_sub, &updated); + + if (updated) { + mission_result_s mission_result; + orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result); + + _current_seq = mission_result.seq_current; + + if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); } + + if (mission_result.reached) { + send_mission_item_reached((uint16_t)mission_result.seq_reached); + } + + send_mission_current(_current_seq); + + } else { + if (_slow_rate_limiter.check(now)) { + send_mission_current(_current_seq); + } + } + + /* check for timed-out operations */ + if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) { + _mavlink->send_statustext("Operation timeout"); + + if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } + + _state = MAVLINK_WPM_STATE_IDLE; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + /* try to request item again after timeout */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + + } else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + if (_transfer_seq == 0) { + /* try to send items count again after timeout */ + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count); + + } else { + /* try to send item again after timeout */ + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1); + } + } +} + + +void +MavlinkMissionManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_MISSION_ACK: + handle_mission_ack(msg); + break; + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: + handle_mission_set_current(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: + handle_mission_request_list(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + handle_mission_request(msg); + break; + + case MAVLINK_MSG_ID_MISSION_COUNT: + handle_mission_count(msg); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: + handle_mission_item(msg); + break; + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: + handle_mission_clear_all(msg); + break; + + default: + break; + } +} + + +void +MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) +{ + mavlink_mission_ack_t wpa; + mavlink_msg_mission_ack_decode(msg, &wpa); + + if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == _count) { + if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + } + + } else { + _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) +{ + mavlink_mission_set_current_t wpc; + mavlink_msg_mission_set_current_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.seq < _count) { + if (update_active_mission(_dataman_id, _count, wpc.seq) == OK) { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); } + + _mavlink->send_statustext("IGN WP CURR CMD: Not in list"); + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } + + _mavlink->send_statustext("IGN WP CURR CMD: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) +{ + mavlink_mission_request_list_t wprl; + mavlink_msg_mission_request_list_decode(msg, &wprl); + + if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_count > 0) { + _state = MAVLINK_WPM_STATE_SENDLIST; + _transfer_seq = 0; + _transfer_count = _count; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } + } + + send_mission_count(msg->sysid, msg->compid, _count); + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } + + _mavlink->send_statustext("IGN REQUEST LIST: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) +{ + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(msg, &wpr); + + if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + /* _transfer_seq contains sequence of expected request */ + if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u", wpr.seq, msg->sysid); } + + _transfer_seq++; + + } else if (wpr.seq == _transfer_seq - 1 && wpr.seq >= 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); } + + } else { + if (_transfer_seq > 0 && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); } + + } else if (_transfer_seq <= 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + return; + } + + /* double check bounds in case of items count changed */ + if (wpr.seq >= 0 && wpr.seq < _count) { + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [0, %u]", wpr.seq, msg->sysid, wpr.seq, _count - 1); } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); } + + _mavlink->send_statustext("IGN MISSION_ITEM_REQUEST: No transfer"); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); } + + _mavlink->send_statustext("REJ. WP CMD: Busy"); + } + + } else { + _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) +{ + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.count > _max_count) { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE); + return; + } + + if (wpc.count == 0) { + if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } + + /* alternate dataman ID anyway to let navigator know about changes */ + update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); + _mavlink->send_statustext("WP COUNT 0: CLEAR MISSION"); + + // TODO send ACK? + return; + } + + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } + + _state = MAVLINK_WPM_STATE_GETLIST; + _transfer_seq = 0; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + _transfer_count = wpc.count; + _transfer_dataman_id = _dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + _transfer_current_seq = -1; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == 0) { + /* looks like our MISSION_REQUEST was lost, try again */ + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } + + _mavlink->send_statustext("WP CMD OK AGAIN"); + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); } + + _mavlink->send_statustext("REJ. WP CMD: Busy"); + return; + } + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); } + + _mavlink->send_statustext("IGN MISSION_COUNT: Busy"); + return; + } + + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } +} + + +void +MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) +{ + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) { + if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (wp.seq != _transfer_seq) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); } + + /* don't send request here, it will be performed in eventloop after timeout */ + return; + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } + + _mavlink->send_statustext("IGN MISSION_ITEM: No transfer"); + return; + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } + + _mavlink->send_statustext("IGN MISSION_ITEM: Busy"); + return; + } + + struct mission_item_s mission_item; + int ret = parse_mavlink_mission_item(&wp, &mission_item); + + if (ret != OK) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + dm_item_t dm_item = _transfer_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + + if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("#audio: Unable to write on micro SD"); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } + + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } + + _transfer_seq = wp.seq + 1; + + if (_transfer_seq == _transfer_count) { + /* got all new mission items successfully */ + if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } + + _state = MAVLINK_WPM_STATE_IDLE; + + if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + /* request next item */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } + } +} + + +void +MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) +{ + mavlink_mission_clear_all_t wpca; + mavlink_msg_mission_clear_all_decode(msg, &wpca); + + if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + + if (_state == MAVLINK_WPM_STATE_IDLE) { + /* don't touch mission items storage itself, but only items count in mission state */ + _time_last_recv = hrt_absolute_time(); + + if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) { + if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + _mavlink->send_statustext("IGN WP CLEAR CMD: Busy"); + + if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } + } + } +} + +int +MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +{ + /* only support global waypoints for now */ + switch (mavlink_mission_item->frame) { + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; + + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; + } + + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param1; + break; + case MAV_CMD_DO_JUMP: + mission_item->do_jump_mission_index = mavlink_mission_item->param1; + mission_item->do_jump_current_count = 0; + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; + break; + default: + mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->time_inside = mavlink_mission_item->param1; + break; + } + + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + 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->autocontinue = mavlink_mission_item->autocontinue; + // mission_item->index = mavlink_mission_item->seq; + mission_item->origin = ORIGIN_MAVLINK; + + /* reset DO_JUMP count */ + mission_item->do_jump_current_count = 0; + + return MAV_MISSION_ACCEPTED; +} + + +int +MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +{ + if (mission_item->altitude_is_relative) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + + switch (mission_item->nav_cmd) { + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param1 = mission_item->pitch_min; + break; + + case NAV_CMD_DO_JUMP: + mavlink_mission_item->param1 = mission_item->do_jump_mission_index; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + break; + + default: + mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param1 = mission_item->time_inside; + break; + } + + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->z = mission_item->altitude; + + 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->autocontinue = mission_item->autocontinue; + // mavlink_mission_item->seq = mission_item->index; + + return OK; +} diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h new file mode 100644 index 000000000..d8695cf83 --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_mission.h + * MAVLink mission manager interface definition. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#pragma once + +#include "mavlink_bridge_header.h" +#include "mavlink_rate_limiter.h" +#include <uORB/uORB.h> + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + +#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds +#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds + +class Mavlink; + +class MavlinkMissionManager { +public: + MavlinkMissionManager(Mavlink *parent); + + ~MavlinkMissionManager(); + + void init_offboard_mission(); + + int update_active_mission(int dataman_id, unsigned count, int seq); + + void send_message(mavlink_message_t *msg); + + /** + * @brief Sends an waypoint ack message + */ + void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type); + + /** + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ + void send_mission_current(uint16_t seq); + + void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count); + + void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq); + + void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq); + + /** + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ + void send_mission_item_reached(uint16_t seq); + + void eventloop(); + + void handle_message(const mavlink_message_t *msg); + + void handle_mission_ack(const mavlink_message_t *msg); + + void handle_mission_set_current(const mavlink_message_t *msg); + + void handle_mission_request_list(const mavlink_message_t *msg); + + void handle_mission_request(const mavlink_message_t *msg); + + void handle_mission_count(const mavlink_message_t *msg); + + void handle_mission_item(const mavlink_message_t *msg); + + void handle_mission_clear_all(const mavlink_message_t *msg); + + /** + * Parse mavlink MISSION_ITEM message to get mission_item_s. + */ + int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); + + /** + * Format mission_item_s as mavlink MISSION_ITEM message. + */ + int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + + void set_verbose(bool v) { _verbose = v; } + +private: + Mavlink* _mavlink; + mavlink_channel_t _channel; + uint8_t _comp_id; + + enum MAVLINK_WPM_STATES _state; ///< Current state + + uint64_t _time_last_recv; + uint64_t _time_last_sent; + + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximal count of mission items + + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission + + int _transfer_dataman_id; ///< Dataman storage ID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + int _transfer_seq; ///< Item sequence in current transmission + int _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission + uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission + + int _offboard_mission_sub; + int _mission_result_sub; + orb_advert_t _offboard_mission_pub; + + MavlinkRateLimiter _slow_rate_limiter; + + bool _verbose; +}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6d361052c..75e4e7160 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -79,7 +79,6 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" #include "mavlink_receiver.h" #include "mavlink_main.h" -#include "util.h" __END_DECLS @@ -969,16 +968,8 @@ MavlinkReceiver::receive_thread(void *arg) /* handle generic messages and commands */ handle_message(&msg); - /* handle packet with waypoint component */ - _mavlink->mavlink_wpm_message_handler(&msg); - - /* handle packet with parameter component */ - _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); - - if (_mavlink->get_forwarding_on()) { - /* forward any messages to other mavlink instances */ - Mavlink::forward_message(&msg, _mavlink); - } + /* handle packet with parent object */ + _mavlink->handle_message(&msg); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 040a07480..80e0ea813 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -102,8 +102,6 @@ public: static void *start_helper(void *context); private: - perf_counter_t _loop_perf; /**< loop performance counter */ - Mavlink *_mavlink; void handle_message(mavlink_message_t *msg); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index a4d8bfbfb..d49bbb7f7 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,6 +39,7 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ + mavlink_mission.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h deleted file mode 100644 index 5ca9a085d..000000000 --- a/src/modules/mavlink/util.h +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file util.h - * Utility and helper functions and data. - */ - -#pragma once - -/** MAVLink communications channel */ -extern uint8_t chan; - -/** Shutdown marker */ -extern volatile bool thread_should_exit; - -/** Waypoint storage */ -extern mavlink_wpm_storage *wpm; - -/** - * Translate the custom state into standard mavlink modes and state. - */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h deleted file mode 100644 index 532eff7aa..000000000 --- a/src/modules/mavlink/waypoints.h +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2009-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file waypoints.h - * MAVLink waypoint protocol definition (BSD-relicensed). - * - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - */ - -#ifndef WAYPOINTS_H_ -#define WAYPOINTS_H_ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - -#include <v1.0/mavlink_types.h> -#include "mavlink_bridge_header.h" -#include <stdbool.h> -#include <uORB/topics/mission.h> - -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; - uint16_t max_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint32_t timeout; - int current_dataman_id; -}; - -typedef struct mavlink_wpm_storage mavlink_wpm_storage; - -int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); -int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - - -void mavlink_wpm_init(mavlink_wpm_storage *state); -void mavlink_waypoint_eventloop(uint64_t now); -void mavlink_wpm_message_handler(const mavlink_message_t *msg); - -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); - -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; - -#endif /* WAYPOINTS_H_ */ diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 72255103b..318ac87b4 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -66,13 +66,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _current_offboard_mission_index(-1), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE) + _mission_type(MISSION_TYPE_NONE), + _inited(false) { /* load initial params */ updateParams(); - /* set initial mission items */ - on_inactive(); - } Mission::~Mission() @@ -84,16 +82,25 @@ Mission::on_inactive() { _first_run = true; - /* check anyway if missions have changed so that feedback to groundstation is given */ - bool onboard_updated; - orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); - if (onboard_updated) { - update_onboard_mission(); - } + if (_inited) { + /* check if missions have changed so that feedback to ground station is given */ + bool onboard_updated = false; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } - bool offboard_updated; - orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); - if (offboard_updated) { + bool offboard_updated = false; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } + + } else { + /* read mission topics on initialization */ + _inited = true; + + update_onboard_mission(); update_offboard_mission(); } } @@ -104,13 +111,13 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool updated = false; /* check if anything has changed */ - bool onboard_updated; + bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); if (onboard_updated) { update_onboard_mission(); } - bool offboard_updated; + bool offboard_updated = false; orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); if (offboard_updated) { update_offboard_mission(); @@ -138,9 +145,9 @@ Mission::update_onboard_mission() { if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { /* accept the current index set by the onboard mission if it is within bounds */ - if (_onboard_mission.current_index >=0 - && _onboard_mission.current_index < (int)_onboard_mission.count) { - _current_onboard_mission_index = _onboard_mission.current_index; + if (_onboard_mission.current_seq >=0 + && _onboard_mission.current_seq < (int)_onboard_mission.count) { + _current_onboard_mission_index = _onboard_mission.current_seq; } else { /* if less WPs available, reset to first WP */ if (_current_onboard_mission_index >= (int)_onboard_mission.count) { @@ -153,7 +160,7 @@ Mission::update_onboard_mission() } } else { _onboard_mission.count = 0; - _onboard_mission.current_index = 0; + _onboard_mission.current_seq = 0; _current_onboard_mission_index = 0; } } @@ -162,13 +169,12 @@ void Mission::update_offboard_mission() { if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { - + warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq); /* determine current index */ - if (_offboard_mission.current_index >= 0 - && _offboard_mission.current_index < (int)_offboard_mission.count) { - _current_offboard_mission_index = _offboard_mission.current_index; + if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) { + _current_offboard_mission_index = _offboard_mission.current_seq; } else { - /* if less WPs available, reset to first WP */ + /* if less items available, reset to first item */ if (_current_offboard_mission_index >= (int)_offboard_mission.count) { _current_offboard_mission_index = 0; /* if not initialized, set it to 0 */ @@ -193,10 +199,12 @@ Mission::update_offboard_mission() _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { + warnx("offboard mission update failed"); _offboard_mission.count = 0; - _offboard_mission.current_index = 0; + _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; } + report_current_offboard_mission_item(); } @@ -315,10 +323,10 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren void Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) { - int next_temp_mission_index = _onboard_mission.current_index + 1; + int next_temp_mission_index = _onboard_mission.current_seq + 1; /* try if there is a next onboard mission */ - if (_onboard_mission.current_index >= 0 && + if (_onboard_mission.current_seq >= 0 && next_temp_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { @@ -338,9 +346,9 @@ void Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) { /* try if there is a next offboard mission */ - int next_temp_mission_index = _offboard_mission.current_index + 1; + int next_temp_mission_index = _offboard_mission.current_seq + 1; warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); - if (_offboard_mission.current_index >= 0 && + if (_offboard_mission.current_seq >= 0 && next_temp_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { @@ -420,11 +428,54 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } void +Mission::save_offboard_mission_state() +{ + mission_s mission_state; + + /* lock MISSION_STATE item */ + dm_lock(DM_KEY_MISSION_STATE); + + /* read current state */ + int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + + if (read_res == sizeof(mission_s)) { + /* data read successfully, check dataman ID and items count */ + if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) { + /* navigator may modify only sequence, write modified state only if it changed */ + if (mission_state.current_seq != _current_offboard_mission_index) { + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); + } + } + } + + } else { + /* invalid data, this must not happen and indicates error in offboard_mission publisher */ + mission_state.dataman_id = _offboard_mission.dataman_id; + mission_state.count = _offboard_mission.count; + mission_state.current_seq = _current_offboard_mission_index; + + warnx("ERROR: invalid mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state"); + + /* write modified state only if changed */ + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); + } + } + + /* unlock MISSION_STATE item */ + dm_unlock(DM_KEY_MISSION_STATE); +} + +void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; + _mission_result.reached = true; + _mission_result.seq_reached = _current_offboard_mission_index; } publish_mission_result(); } @@ -432,14 +483,17 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _current_offboard_mission_index; + warnx("current offboard mission index: %d", _current_offboard_mission_index); + _mission_result.seq_current = _current_offboard_mission_index; publish_mission_result(); + + save_offboard_mission_state(); } void Mission::report_mission_finished() { - _mission_result.mission_finished = true; + _mission_result.finished = true; publish_mission_result(); } @@ -456,6 +510,6 @@ Mission::publish_mission_result() _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } /* reset reached bool */ - _mission_result.mission_reached = false; - _mission_result.mission_finished = false; + _mission_result.reached = false; + _mission_result.finished = false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6e4761946..7f8cdf528 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -136,6 +136,11 @@ private: struct mission_item_s *new_mission_item); /** + * Save current offboard mission state to dataman + */ + void save_offboard_mission_state(); + + /** * Report that a mission item has been reached */ void report_mission_item_reached(); @@ -172,6 +177,8 @@ private: MISSION_TYPE_OFFBOARD } _mission_type; + bool _inited; + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index d9dd61df1..e4b72f87c 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -98,11 +98,15 @@ struct mission_item_s { unsigned do_jump_current_count; /**< count how many times the jump has been done */ }; +/** + * This topic used to notify navigator about mission changes, mission itself and new mission state + * must be stored in dataman before publication. + */ struct mission_s { - int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */ - unsigned count; /**< count of the missions stored in the datamanager */ - int current_index; /**< default -1, start at the one changed latest */ + int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ + unsigned count; /**< count of the missions stored in the dataman */ + int current_seq; /**< default -1, start at the one changed latest */ }; /** diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index ad654a9ff..beb797e62 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -53,10 +53,10 @@ struct mission_result_s { - bool mission_reached; /**< true if mission has been reached */ - unsigned mission_index_reached; /**< index of the mission which has been reached */ - unsigned index_current_mission; /**< index of the current mission */ - bool mission_finished; /**< true if mission has been completed */ + unsigned seq_reached; /**< Sequence of the mission item which has been reached */ + unsigned seq_current; /**< Sequence of the current mission item */ + bool reached; /**< true if mission has been reached */ + bool finished; /**< true if mission has been completed */ }; /** |