From fbb3adde06e5ecf88a4c39e332a539fa12d173b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 16:04:02 +0200 Subject: mavlink app: Clean up allocations --- src/modules/mavlink/mavlink_main.cpp | 112 ++++++++++------------------------- 1 file changed, 32 insertions(+), 80 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6c97bfca7..340b20e1b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -234,6 +234,11 @@ Mavlink::Mavlink() : _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), _message_buffer({}), + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -493,44 +498,39 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) void Mavlink::mavlink_update_system(void) { - static bool initialized = false; - static param_t param_system_id; - static param_t param_component_id; - static param_t param_system_type; - static param_t param_use_hil_gps; - if (!initialized) { - param_system_id = param_find("MAV_SYS_ID"); - param_component_id = param_find("MAV_COMP_ID"); - param_system_type = param_find("MAV_TYPE"); - param_use_hil_gps = param_find("MAV_USEHILGPS"); - initialized = true; + if (!_param_initialized) { + _param_system_id = param_find("MAV_SYS_ID"); + _param_component_id = param_find("MAV_COMP_ID"); + _param_system_type = param_find("MAV_TYPE"); + _param_use_hil_gps = param_find("MAV_USEHILGPS"); + _param_initialized = true; } /* update system and component id */ int32_t system_id; - param_get(param_system_id, &system_id); + param_get(_param_system_id, &system_id); if (system_id > 0 && system_id < 255) { mavlink_system.sysid = system_id; } int32_t component_id; - param_get(param_component_id, &component_id); + param_get(_param_component_id, &component_id); if (component_id > 0 && component_id < 255) { mavlink_system.compid = component_id; } int32_t system_type; - param_get(param_system_type, &system_type); + param_get(_param_system_type, &system_type); if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { mavlink_system.type = system_type; } int32_t use_hil_gps; - param_get(param_use_hil_gps, &use_hil_gps); + param_get(_param_use_hil_gps, &use_hil_gps); _use_hil_gps = (bool)use_hil_gps; } @@ -791,7 +791,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* Start sending parameters */ mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + mavlink_missionlib_send_gcs_string("[pm] sending list"); } break; case MAVLINK_MSG_ID_PARAM_SET: { @@ -813,7 +813,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, "[mavlink pm] unknown: %s", name); + sprintf(buf, "[pm] unknown: %s", name); mavlink_missionlib_send_gcs_string(buf); } else { @@ -1001,8 +1001,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) } else { mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - - if (_verbose) { warnx("ERROR: index out of bounds"); } } } @@ -1073,8 +1071,6 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u } else { mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - - if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); } } } @@ -1105,8 +1101,6 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) 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); } - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; _wpm->current_partner_sysid = 0; _wpm->current_partner_compid = 0; @@ -1137,8 +1131,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); } } break; @@ -1162,21 +1154,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - - if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); } } } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - - if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } - } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } } break; @@ -1206,14 +1191,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - - if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } } } else { mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - - if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } } break; @@ -1230,8 +1211,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); } - break; } @@ -1242,15 +1221,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); } + 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"); - if (_verbose) { warnx("REJ. WP CMD: First id != 0"); } - break; } @@ -1258,17 +1235,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); } + 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 MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + 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"); - 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; } @@ -1276,8 +1251,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); } - break; } @@ -1291,7 +1264,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { 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); } + mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds"); } @@ -1301,13 +1274,9 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } - } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } @@ -1331,15 +1300,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } if (wpc.count == 0) { - mavlink_missionlib_send_gcs_string("COUNT 0"); - - if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); } + mavlink_missionlib_send_gcs_string("WP COUNT 0"); break; } - 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; @@ -1353,25 +1318,17 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); } - } 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); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP"); } } else { mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - - if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } } } else { mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } break; @@ -1393,7 +1350,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wp.seq != 0) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); break; } @@ -1401,12 +1357,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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_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; } @@ -1473,8 +1428,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1515,8 +1468,6 @@ 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) { mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1535,8 +1486,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - - mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); + mavlink_send_uart_bytes(_channel, buf, len); } @@ -1619,6 +1569,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (interval > 0) { /* search for stream with specified name in supported streams list */ for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { /* create new instance */ stream = streams_list[i]->new_instance(); @@ -1924,7 +1875,7 @@ Mavlink::task_main(int argc, char *argv[]) /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ if (_passing_on) { /* initialize message buffer if multiplexing is on */ - if (OK != message_buffer_init(500)) { + if (OK != message_buffer_init(300)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1956,7 +1907,8 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); - struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + struct vehicle_status_s status; + status_sub->update(0, &status); MavlinkCommandsStream commands_stream(this, _channel); @@ -2013,14 +1965,14 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); - if (param_sub->update(t)) { + if (param_sub->update(t, nullptr)) { /* parameters updated */ mavlink_update_system(); } - if (status_sub->update(t)) { + if (status_sub->update(t, &status)) { /* switch HIL mode if required */ - set_hil_enabled(status->hil_state == HIL_STATE_ON); + set_hil_enabled(status.hil_state == HIL_STATE_ON); } /* update commands stream */ -- cgit v1.2.3