dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink app: Clean up allocations

This commit is contained in:
Lorenz Meier 2014-05-13 16:04:02 +02:00
parent a72015c260
commit fbb3adde06
9 changed files with 1080 additions and 1074 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/mavlink/mavlink_commands.cpp
View File

@ -43,7 +43,6 @@
MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
{
_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
_cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
}
MavlinkCommandsStream::~MavlinkCommandsStream()
@ -53,21 +52,23 @@ MavlinkCommandsStream::~MavlinkCommandsStream()
void
MavlinkCommandsStream::update(const hrt_abstime t)
{
if (_cmd_sub->update(t)) {
struct vehicle_command_s cmd;
if (_cmd_sub->update(t, &cmd)) {
/* only send commands for other systems/components */
if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) {
if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
mavlink_msg_command_long_send(_channel,
_cmd->target_system,
_cmd->target_component,
_cmd->command,
_cmd->confirmation,
_cmd->param1,
_cmd->param2,
_cmd->param3,
_cmd->param4,
_cmd->param5,
_cmd->param6,
_cmd->param7);
cmd.target_system,
cmd.target_component,
cmd.command,
cmd.confirmation,
cmd.param1,
cmd.param2,
cmd.param3,
cmd.param4,
cmd.param5,
cmd.param6,
cmd.param7);
}
}
}

112
src/modules/mavlink/mavlink_main.cpp
View File

@ -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 */

10
src/modules/mavlink/mavlink_main.h
View File

@ -278,11 +278,15 @@ private:
int size;
char *data;
};
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
bool _param_initialized;
param_t _param_system_id;
param_t _param_component_id;
param_t _param_system_type;
param_t _param_use_hil_gps;
/**
* Send one parameter.

1928
src/modules/mavlink/mavlink_messages.cpp
View File

File diff suppressed because it is too large Load Diff

15
src/modules/mavlink/mavlink_messages.h
View File

@ -43,6 +43,19 @@
#include "mavlink_stream.h"
extern MavlinkStream *streams_list[];
class StreamListItem {
public:
MavlinkStream* (*new_instance)();
const char* (*get_name)();
StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
new_instance(inst),
get_name(name) {};
~StreamListItem() {};
};
extern StreamListItem *streams_list[];
#endif /* MAVLINK_MESSAGES_H_ */

19
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -53,30 +53,21 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
_last_check(0),
next(nullptr)
{
_data = malloc(topic->o_size);
memset(_data, 0, topic->o_size);
}
MavlinkOrbSubscription::~MavlinkOrbSubscription()
{
close(_fd);
free(_data);
}
const orb_id_t
MavlinkOrbSubscription::get_topic()
orb_id_t
MavlinkOrbSubscription::get_topic() const
{
return _topic;
}
void *
MavlinkOrbSubscription::get_data()
{
return _data;
}
bool
MavlinkOrbSubscription::update(const hrt_abstime t)
MavlinkOrbSubscription::update(const hrt_abstime t, void* data)
{
if (_last_check == t) {
/* already checked right now, return result of the check */
@ -86,8 +77,8 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
_last_check = t;
orb_check(_fd, &_updated);
if (_updated) {
orb_copy(_topic, _fd, _data);
if (_updated && data) {
orb_copy(_topic, _fd, data);
_published = true;
return true;
}

18
src/modules/mavlink/mavlink_orb_subscription.h
View File

@ -48,12 +48,12 @@
class MavlinkOrbSubscription
{
public:
MavlinkOrbSubscription *next; /*< pointer to next subscription in list */
MavlinkOrbSubscription *next; ///< pointer to next subscription in list
MavlinkOrbSubscription(const orb_id_t topic);
~MavlinkOrbSubscription();
bool update(const hrt_abstime t);
bool update(const hrt_abstime t, void* data);
/**
* Check if the topic has been published.
@ -62,16 +62,14 @@ public:
* @return true if the topic has been published at least once.
*/
bool is_published();
void *get_data();
const orb_id_t get_topic();
orb_id_t get_topic() const;
private:
const orb_id_t _topic; /*< topic metadata */
int _fd; /*< subscription handle */
bool _published; /*< topic was ever published */
void *_data; /*< pointer to data buffer */
hrt_abstime _last_check; /*< time of last check */
bool _updated; /*< updated on last check */
const orb_id_t _topic; ///< topic metadata
int _fd; ///< subscription handle
bool _published; ///< topic was ever published
hrt_abstime _last_check; ///< time of last check
bool _updated; ///< updated on last check
};

1
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -950,7 +950,6 @@ MavlinkReceiver::receive_start(Mavlink *parent)
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2900);
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);

22
src/modules/mavlink/mavlink_stream.h
View File

@ -50,14 +50,6 @@ class MavlinkStream;
class MavlinkStream
{
private:
hrt_abstime _last_sent;
protected:
mavlink_channel_t _channel;
unsigned int _interval;
virtual void send(const hrt_abstime t) = 0;
public:
MavlinkStream *next;
@ -67,9 +59,19 @@ public:
void set_interval(const unsigned int interval);
void set_channel(mavlink_channel_t channel);
int update(const hrt_abstime t);
virtual MavlinkStream *new_instance() = 0;
static MavlinkStream *new_instance();
static const char *get_name_static();
virtual void subscribe(Mavlink *mavlink) = 0;
virtual const char *get_name() = 0;
virtual const char *get_name() const = 0;
protected:
mavlink_channel_t _channel;
unsigned int _interval;
virtual void send(const hrt_abstime t) = 0;
private:
hrt_abstime _last_sent;
};