diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 17f7dd077d..c96f44e023 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -118,6 +118,7 @@ then set MKBLCTRL_MODE none set FMU_MODE pwm set MAVLINK_FLAGS default + set EXIT_ON_END no set MAV_TYPE none # @@ -380,7 +381,6 @@ then # # MAVLink # - set EXIT_ON_END no if [ $MAVLINK_FLAGS == default ] then @@ -539,6 +539,7 @@ then if [ $EXIT_ON_END == yes ] then + echo "[init] Exit from nsh" exit fi diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 568666c1ea..99bef1f731 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -92,7 +92,7 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz -static Mavlink *_head = nullptr; +static Mavlink *_mavlink_instances = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; @@ -160,35 +160,30 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); Mavlink::Mavlink() : - device_name(DEFAULT_DEVICE_NAME), - _task_should_exit(false), next(nullptr), + _device_name(DEFAULT_DEVICE_NAME), + _task_should_exit(false), _mavlink_fd(-1), - thread_running(false), - _mavlink_task(-1), - _mavlink_incoming_fd(-1), + _task_running(false), _mavlink_hil_enabled(false), _subscriptions(nullptr), _streams(nullptr), - mission_pub(-1), - mavlink_param_queue_index(0), + _mission_pub(-1), + _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { - wpm = &wpm_s; + _wpm = &_wpm_s; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; - // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); - } Mavlink::~Mavlink() { - if (_mavlink_task != -1) { - - /* task wakes up every 100ms or so at the longest */ + if (_task_running) { + /* task wakes up every 10ms or so at the longest */ _task_should_exit = true; /* wait for a second for the task to quit at our request */ @@ -200,10 +195,11 @@ Mavlink::~Mavlink() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_mavlink_task); + //TODO store main task handle in Mavlink instance to allow killing task + //task_delete(_mavlink_task); break; } - } while (_mavlink_task != -1); + } while (_task_running); } } @@ -216,12 +212,10 @@ Mavlink::set_mode(enum MAVLINK_MODE mode) int Mavlink::instance_count() { - /* note: a local buffer count will help if this ever is called often */ - Mavlink *inst = ::_head; unsigned inst_index = 0; + Mavlink *inst; - while (inst != nullptr) { - inst = inst->next; + LL_FOREACH(::_mavlink_instances, inst) { inst_index++; } @@ -232,11 +226,11 @@ Mavlink * Mavlink::new_instance() { Mavlink *inst = new Mavlink(); - Mavlink *next = ::_head; + Mavlink *next = ::_mavlink_instances; /* create the first instance at _head */ - if (::_head == nullptr) { - ::_head = inst; + if (::_mavlink_instances == nullptr) { + ::_mavlink_instances = inst; /* afterwards follow the next and append the instance */ } else { @@ -254,19 +248,17 @@ Mavlink::new_instance() Mavlink * Mavlink::get_instance(unsigned instance) { - Mavlink *inst = ::_head; + Mavlink *inst; unsigned inst_index = 0; + LL_FOREACH(::_mavlink_instances, inst) { + if (instance == inst_index) { + return inst; + } - while (inst->next != nullptr && inst_index < instance) { - inst = inst->next; inst_index++; } - if (inst_index < instance) { - inst = nullptr; - } - - return inst; + return nullptr; } Mavlink * @@ -274,8 +266,8 @@ Mavlink::get_instance_for_device(const char *device_name) { Mavlink *inst; - LL_FOREACH(::_head, inst) { - if (strcmp(inst->device_name, device_name) == 0) { + LL_FOREACH(::_mavlink_instances, inst) { + if (strcmp(inst->_device_name, device_name) == 0) { return inst; } } @@ -288,21 +280,20 @@ Mavlink::destroy_all_instances() { /* start deleting from the end */ Mavlink *inst_to_del = nullptr; - Mavlink *next_inst = ::_head; + Mavlink *next_inst = ::_mavlink_instances; unsigned iterations = 0; warnx("waiting for instances to stop"); while (next_inst != nullptr) { - inst_to_del = next_inst; next_inst = inst_to_del->next; /* set flag to stop thread and wait for all threads to finish */ inst_to_del->_task_should_exit = true; - while (inst_to_del->thread_running) { + while (inst_to_del->_task_running) { printf("."); fflush(stdout); usleep(10000); @@ -318,7 +309,7 @@ Mavlink::destroy_all_instances() } /* reset head */ - ::_head = nullptr; + ::_mavlink_instances = nullptr; printf("\n"); warnx("all instances stopped"); @@ -328,12 +319,12 @@ Mavlink::destroy_all_instances() bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { - Mavlink *inst = ::_head; + Mavlink *inst = ::_mavlink_instances; while (inst != nullptr) { /* don't compare with itself */ - if (inst != self && !strcmp(device_name, inst->device_name)) { + if (inst != self && !strcmp(device_name, inst->_device_name)) { return true; } @@ -358,7 +349,7 @@ Mavlink::get_uart_fd(unsigned index) int Mavlink::get_uart_fd() { - return _uart; + return _uart_fd; } mavlink_channel_t @@ -384,12 +375,12 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - Mavlink *inst = ::_head; + Mavlink *inst = ::_mavlink_instances; while (inst != nullptr) { - mavlink_logbuffer_write(&inst->lb, &msg); - inst->total_counter++; + mavlink_logbuffer_write(&inst->_logbuffer, &msg); + inst->_total_counter++; inst = inst->next; } @@ -493,7 +484,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } /* open uart */ - _uart = open(uart_name, O_RDWR | O_NOCTTY); + _uart_fd = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ struct termios uart_config; @@ -501,14 +492,14 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * *is_usb = false; /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(_uart, uart_config_original)) < 0) { + if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); - close(_uart); + close(_uart_fd); return -1; } /* Fill the struct for the new configuration */ - tcgetattr(_uart, &uart_config); + tcgetattr(_uart_fd, &uart_config); /* Clear ONLCR flag (which appends a CR for every LF) */ uart_config.c_oflag &= ~ONLCR; @@ -519,19 +510,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); - close(_uart); + close(_uart_fd); return -1; } } - if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) { + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(_uart); + close(_uart_fd); return -1; } - return _uart; + return _uart_fd; } int @@ -583,9 +574,9 @@ extern mavlink_system_t mavlink_system; int Mavlink::mavlink_pm_queued_send() { - if (mavlink_param_queue_index < param_count()) { - mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); - mavlink_param_queue_index++; + if (_mavlink_param_queue_index < param_count()) { + mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); + _mavlink_param_queue_index++; return 0; } else { @@ -595,7 +586,7 @@ int Mavlink::mavlink_pm_queued_send() void Mavlink::mavlink_pm_start_queued_send() { - mavlink_param_queue_index = 0; + _mavlink_param_queue_index = 0; } int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) @@ -730,11 +721,11 @@ 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(mission), &mission); + if (_mission_pub < 0) { + _mission_pub = orb_advertise(ORB_ID(mission), &mission); } else { - orb_publish(ORB_ID(mission), mission_pub, &mission); + orb_publish(ORB_ID(mission), _mission_pub, &mission); } } @@ -863,7 +854,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 */ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) { - if (seq < wpm->size) { + if (seq < _wpm->size) { mavlink_message_t msg; mavlink_mission_current_t wpc; @@ -872,7 +863,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - } else if (seq == 0 && wpm->size == 0) { + } else if (seq == 0 && _wpm->size == 0) { /* don't broadcast if no WPs */ @@ -906,7 +897,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t dm_item_t dm_current; - if (wpm->current_dataman_id == 0) { + if (_wpm->current_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { @@ -929,7 +920,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t 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_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); if (_verbose) { warnx("ERROR: could not read WP%u", seq); } } @@ -937,7 +928,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) { - if (seq < wpm->max_size) { + if (seq < _wpm->max_size) { mavlink_message_t msg; mavlink_mission_request_t wpr; wpr.target_system = sysid; @@ -978,15 +969,15 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t 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) { + if (now - _wpm->timestamp_lastaction > _wpm->timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("Operation timeout"); - if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); } + if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_partner_sysid = 0; - wpm->current_partner_compid = 0; + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_partner_sysid = 0; + _wpm->current_partner_compid = 0; } } @@ -1001,14 +992,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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 ((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) { + 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; + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_wp_id = 0; } } @@ -1026,10 +1017,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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; + _wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < wpm->size) { + if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + if (wpc.seq < _wpm->size) { mission.current_index = wpc.seq; publish_mission(); @@ -1064,22 +1055,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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; + _wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpm->size > 0) { + 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; + _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); + _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"); @@ -1100,10 +1091,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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 (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) { + if (wpr.seq >= _wpm->size) { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); @@ -1116,12 +1107,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) * 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 (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpr.seq == 0) { if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); @@ -1131,20 +1122,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq == wpm->current_wp_id) { + if (wpr.seq == _wpm->current_wp_id) { if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - } else if (wpr.seq == wpm->current_wp_id + 1) { + } else if (wpr.seq == _wpm->current_wp_id + 1) { if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); } + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); } break; } @@ -1153,20 +1144,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); } + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); } break; } - wpm->current_wp_id = wpr.seq; - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + _wpm->current_wp_id = wpr.seq; + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - if (wpr.seq < wpm->size) { + if (wpr.seq < _wpm->size) { - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + 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_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } } @@ -1174,11 +1165,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } 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)) { + 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); } + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } } else { @@ -1196,14 +1187,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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; + _wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + 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); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); break; } @@ -1217,17 +1208,17 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - 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; + _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); + 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) { + } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wpm->current_wp_id == 0) { + if (_wpm->current_wp_id == 0) { mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } @@ -1235,7 +1226,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); } + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); } } } else { @@ -1259,14 +1250,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - wpm->timestamp_lastaction = now; + _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 (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { if (wp.seq != 0) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); @@ -1274,30 +1265,30 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (wp.seq >= wpm->current_count) { + if (wp.seq >= _wpm->current_count) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); break; } - if (wp.seq != wpm->current_wp_id) { - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, wpm->current_wp_id); - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + if (wp.seq != _wpm->current_wp_id) { + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id); + 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; + _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; + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } @@ -1305,7 +1296,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) dm_item_t dm_next; - if (wpm->current_dataman_id == 0) { + if (_wpm->current_dataman_id == 0) { dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; mission.dataman_id = 1; @@ -1315,8 +1306,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } @@ -1327,25 +1318,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) // XXX ignore current set mission.current_index = -1; - wpm->current_wp_id = wp.seq + 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 (_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); } + 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); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - mission.count = wpm->current_count; + mission.count = _wpm->current_count; publish_mission(); - wpm->current_dataman_id = mission.dataman_id; - wpm->size = wpm->current_count; + _wpm->current_dataman_id = mission.dataman_id; + _wpm->size = _wpm->current_count; - wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _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); + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } else { @@ -1363,10 +1354,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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; + if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + _wpm->timestamp_lastaction = now; - wpm->size = 0; + _wpm->size = 0; /* prepare mission topic */ mission.dataman_id = -1; @@ -1375,10 +1366,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + 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); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); } @@ -1389,7 +1380,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); @@ -1549,7 +1540,7 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 5); + mavlink_logbuffer_init(&_logbuffer, 5); int ch; _baudrate = 57600; @@ -1589,7 +1580,7 @@ Mavlink::task_main(int argc, char *argv[]) break; case 'd': - device_name = optarg; + _device_name = optarg; break; // case 'e': @@ -1636,8 +1627,8 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = MAX_DATA_RATE; } - if (Mavlink::instance_exists(device_name, this)) { - errx(1, "mavlink instance for %s already running", device_name); + if (Mavlink::instance_exists(_device_name, this)) { + errx(1, "mavlink instance for %s already running", _device_name); } /* inform about mode */ @@ -1680,7 +1671,7 @@ Mavlink::task_main(int argc, char *argv[]) } warnx("data rate: %d bytes/s", _datarate); - warnx("port: %s, baudrate: %d", device_name, _baudrate); + warnx("port: %s, baudrate: %d", _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1689,16 +1680,14 @@ Mavlink::task_main(int argc, char *argv[]) bool usb_uart; /* default values for arguments */ - _uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); + _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &usb_uart); - if (_uart < 0) { - err(1, "could not open %s", device_name); + if (_uart_fd < 0) { + err(1, "could not open %s", _device_name); } /* create the device node that's used for sending text log messages, etc. */ - if (instance_count() == 1) { - register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); - } + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); /* initialize logging device */ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -1707,16 +1696,16 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ - receive_thread = MavlinkReceiver::receive_start(this); + _receive_thread = MavlinkReceiver::receive_start(this); /* initialize waypoint manager */ - mavlink_wpm_init(wpm); + mavlink_wpm_init(_wpm); int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); struct mission_result_s mission_result; memset(&mission_result, 0, sizeof(mission_result)); - thread_running = true; + _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); @@ -1759,7 +1748,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* don't send parameters on startup without request */ - mavlink_param_queue_index = param_count(); + _mavlink_param_queue_index = param_count(); MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); @@ -1791,15 +1780,16 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, device_name, _subscribe_to_stream_rate); + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, _subscribe_to_stream_rate); } else { - warnx("stream %s on device %s disabled", _subscribe_to_stream, device_name); + warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); } } else { - warnx("stream %s not found", _subscribe_to_stream, device_name); + warnx("stream %s not found", _subscribe_to_stream, _device_name); } + delete _subscribe_to_stream; _subscribe_to_stream = nullptr; } @@ -1834,9 +1824,9 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_pm_queued_send(); mavlink_waypoint_eventloop(hrt_absolute_time()); - if (!mavlink_logbuffer_is_empty(&lb)) { + if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&lb, &msg); + int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { mavlink_missionlib_send_gcs_string(msg.text); @@ -1850,25 +1840,23 @@ Mavlink::task_main(int argc, char *argv[]) delete _subscribe_to_stream; _subscribe_to_stream = nullptr; - warnv("waiting for UART receive thread"); + warnx("waiting for UART receive thread"); /* wait for threads to complete */ - pthread_join(receive_thread, NULL); + pthread_join(_receive_thread, NULL); /* reset the UART flags to original state */ - tcsetattr(_uart, TCSANOW, &uart_config_original); + tcsetattr(_uart_fd, TCSANOW, &uart_config_original); /* close UART */ - close(_uart); + close(_uart_fd); /* destroy log buffer */ - mavlink_logbuffer_destroy(&lb); - - thread_running = false; + mavlink_logbuffer_destroy(&_logbuffer); warnx("exiting"); - _mavlink_task = -1; + _task_running = false; _exit(0); } @@ -1876,6 +1864,7 @@ int Mavlink::start_helper(int argc, char *argv[]) { /* create the instance in task context */ Mavlink *instance = Mavlink::new_instance(); + /* this will actually only return once MAVLink exits */ return instance->task_main(argc, argv); } @@ -1887,37 +1876,12 @@ Mavlink::start(int argc, char *argv[]) char buf[32]; sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); - /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); - - // while (!this->is_running()) { - // usleep(200); - // } - - // if (mavlink->_mavlink_task < 0) { - // warn("task start failed"); - // return -errno; - // } - - // if (mavlink::g_mavlink != nullptr) { - // errx(1, "already running"); - // } - - // mavlink::g_mavlink = new Mavlink; - - // if (mavlink::g_mavlink == nullptr) { - // errx(1, "alloc failed"); - // } - - // if (OK != mavlink::g_mavlink->start()) { - // delete mavlink::g_mavlink; - // mavlink::g_mavlink = nullptr; - // err(1, "start failed"); - // } + task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); return OK; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ebea53d52e..94bef7e726 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -142,7 +142,7 @@ public: int get_uart_fd(); - const char *device_name; + const char *_device_name; enum MAVLINK_MODE { MODE_CUSTOM = 0, @@ -191,10 +191,7 @@ protected: private: int _mavlink_fd; - bool thread_running; - int _mavlink_task; /**< task handle for sensor task */ - - int _mavlink_incoming_fd; /**< file descriptor on which to receive incoming strings */ + bool _task_running; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -204,7 +201,7 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - orb_advert_t mission_pub; + orb_advert_t _mission_pub; struct mission_s mission; uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)]; MAVLINK_MODE _mode; @@ -212,17 +209,17 @@ private: uint8_t _mavlink_wpm_comp_id; mavlink_channel_t _channel; - struct mavlink_logbuffer lb; - unsigned int total_counter; + struct mavlink_logbuffer _logbuffer; + unsigned int _total_counter; - pthread_t receive_thread; + pthread_t _receive_thread; /* Allocate storage space for waypoints */ - mavlink_wpm_storage wpm_s; - mavlink_wpm_storage *wpm; + mavlink_wpm_storage _wpm_s; + mavlink_wpm_storage *_wpm; bool _verbose; - int _uart; + int _uart_fd; int _baudrate; int _datarate; @@ -231,11 +228,11 @@ private: * logic will send parameters from the current index * to len - 1, the end of the param list. */ - unsigned int mavlink_param_queue_index; + unsigned int _mavlink_param_queue_index; bool mavlink_link_termination_allowed; - char * _subscribe_to_stream; + char *_subscribe_to_stream; float _subscribe_to_stream_rate; /**