fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:18:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink_receiver: merge handle_message_command_{long,int} into a common method

Browse Source
This commit is contained in:
Beat Küng
2017-10-31 22:16:06 +01:00
committed by Lorenz Meier
parent 29af2343e1
commit 8f7a5d0e0c
2 changed files with 55 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+49 -105
View File
@@ -450,88 +450,25 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
mavlink_command_long_t cmd_mavlink; mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink); mavlink_msg_command_long_decode(msg, &cmd_mavlink);
bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component); struct vehicle_command_s vcmd = {
.timestamp = hrt_absolute_time(),
bool send_ack = true; .param5 = cmd_mavlink.param5,
int ret = PX4_OK; .param6 = cmd_mavlink.param6,
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
if (!target_ok) { .param1 = cmd_mavlink.param1,
ret = PX4_ERROR; .param2 = cmd_mavlink.param2,
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret); .param3 = cmd_mavlink.param3,
return; .param4 = cmd_mavlink.param4,
} .param7 = cmd_mavlink.param7,
.command = cmd_mavlink.command,
if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { .target_system = cmd_mavlink.target_system,
/* send autopilot version message */ .target_component = cmd_mavlink.target_component,
_mavlink->send_autopilot_capabilites(); .source_system = msg->sysid,
.source_component = msg->compid,
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) { .confirmation = cmd_mavlink.confirmation,
/* send protocol version message */ .from_external = true
_mavlink->send_protocol_version(); };
handle_message_command_both(msg, cmd_mavlink, vcmd);
} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
ret = set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3);
} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
get_message_interval((int)cmd_mavlink.param1);
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_FLIGHT_INFORMATION) {
send_flight_information();
} else {
send_ack = false;
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
PX4_WARN("ignoring CMD with same SYS/COMP (%d/%d) ID", mavlink_system.sysid, mavlink_system.compid);
return;
}
if (cmd_mavlink.command == MAV_CMD_LOGGING_START) {
// we already instanciate the streaming object, because at this point we know on which
// mavlink channel streaming was requested. But in fact it's possible that the logger is
// not even running. The main mavlink thread takes care of this by waiting for an ack
// from the logger.
_mavlink->try_start_ulog_streaming(msg->sysid, msg->compid);
} else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) {
_mavlink->request_stop_ulog_streaming();
}
struct vehicle_command_s vcmd = {
.timestamp = hrt_absolute_time(),
.param5 = cmd_mavlink.param5,
.param6 = cmd_mavlink.param6,
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
.param1 = cmd_mavlink.param1,
.param2 = cmd_mavlink.param2,
.param3 = cmd_mavlink.param3,
.param4 = cmd_mavlink.param4,
.param7 = cmd_mavlink.param7,
// XXX do proper translation
.command = cmd_mavlink.command,
.target_system = cmd_mavlink.target_system,
.target_component = cmd_mavlink.target_component,
.source_system = msg->sysid,
.source_component = msg->compid,
.confirmation = cmd_mavlink.confirmation,
.from_external = true
};
if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
}
}
if (send_ack) {
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret);
}
} }
void void
@@ -541,6 +478,34 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
mavlink_command_int_t cmd_mavlink; mavlink_command_int_t cmd_mavlink;
mavlink_msg_command_int_decode(msg, &cmd_mavlink); mavlink_msg_command_int_decode(msg, &cmd_mavlink);
struct vehicle_command_s vcmd = {
.timestamp = hrt_absolute_time(),
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
.param5 = ((double)cmd_mavlink.x) / 1e7,
.param6 = ((double)cmd_mavlink.y) / 1e7,
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
.param1 = cmd_mavlink.param1,
.param2 = cmd_mavlink.param2,
.param3 = cmd_mavlink.param3,
.param4 = cmd_mavlink.param4,
.param7 = cmd_mavlink.z,
.command = cmd_mavlink.command,
.target_system = cmd_mavlink.target_system,
.target_component = cmd_mavlink.target_component,
.source_system = msg->sysid,
.source_component = msg->compid,
.confirmation = false,
.from_external = true
};
handle_message_command_both(msg, cmd_mavlink, vcmd);
}
template <class T>
void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
const vehicle_command_s &vehicle_command)
{
bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component); bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
bool send_ack = true; bool send_ack = true;
@@ -592,32 +557,11 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
_mavlink->request_stop_ulog_streaming(); _mavlink->request_stop_ulog_streaming();
} }
struct vehicle_command_s vcmd = {
.timestamp = hrt_absolute_time(),
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
.param5 = ((double)cmd_mavlink.x) / 1e7,
.param6 = ((double)cmd_mavlink.y) / 1e7,
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
.param1 = cmd_mavlink.param1,
.param2 = cmd_mavlink.param2,
.param3 = cmd_mavlink.param3,
.param4 = cmd_mavlink.param4,
.param7 = cmd_mavlink.z,
// XXX do proper translation
.command = cmd_mavlink.command,
.target_system = cmd_mavlink.target_system,
.target_component = cmd_mavlink.target_component,
.source_system = msg->sysid,
.source_component = msg->compid,
.confirmation = false,
.from_external = true
};
if (_cmd_pub == nullptr) { if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, vehicle_command_s::ORB_QUEUE_LENGTH);
} else { } else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vehicle_command);
} }
} }
src/modules/mavlink/mavlink_receiver.h
+6
View File
@@ -119,6 +119,12 @@ private:
void handle_message(mavlink_message_t *msg); void handle_message(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg);
void handle_message_command_int(mavlink_message_t *msg); void handle_message_command_int(mavlink_message_t *msg);
/**
* common method to handle both mavlink command types. T is one of mavlink_command_int_t or mavlink_command_long_t
*/
template<class T>
void handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
const vehicle_command_s &vehicle_command);
void handle_message_command_ack(mavlink_message_t *msg); void handle_message_command_ack(mavlink_message_t *msg);
void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_optical_flow_rad(mavlink_message_t *msg);
void handle_message_hil_optical_flow(mavlink_message_t *msg); void handle_message_hil_optical_flow(mavlink_message_t *msg);