fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 21:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into obcfailsafe

Browse Source 
Conflicts:
	src/modules/commander/commander.cpp
This commit is contained in:
Thomas Gubler
2014-08-21 06:31:22 +02:00
parent 760a7ff548 24b1ff23f2
commit 0f01905f9a
13 changed files with 810 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+4
View File
@@ -217,6 +217,8 @@ Mavlink::Mavlink() :
errx(1, "instance ID is out of range");
break;
}
_rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
}
Mavlink::~Mavlink()
@@ -1653,6 +1655,8 @@ Mavlink::display_status()
printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
}
printf("\tmavlink chan: #%u\n", _channel);
if (_rstatus.timestamp > 0) {
printf("\ttype:\t\t");
src/modules/mavlink/mavlink_receiver.cpp
+69 -21
View File
@@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
_radio_status_available(false),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@@ -137,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_command_long(msg);
break;
case MAVLINK_MSG_ID_COMMAND_INT:
handle_message_command_int(msg);
break;
case MAVLINK_MSG_ID_OPTICAL_FLOW:
handle_message_optical_flow(msg);
break;
@@ -276,6 +279,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
{
/* command */
mavlink_command_int_t cmd_mavlink;
mavlink_msg_command_int_decode(msg, &cmd_mavlink);
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */
warnx("terminated by remote command");
fflush(stdout);
usleep(50000);
/* terminate other threads and this thread */
_mavlink->_task_should_exit = true;
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
return;
}
struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd));
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4;
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
vcmd.param7 = cmd_mavlink.z;
// XXX do proper translation
vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
}
}
}
}
void
MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
{
@@ -430,9 +489,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
/* this means that heartbeats alone won't be published to the radio status no more */
_radio_status_available = true;
}
}
@@ -474,25 +530,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
hrt_abstime tnow = hrt_absolute_time();
/* set heartbeat time and topic time and publish -
* the telem status also gets updated on telemetry events
*/
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = tstatus.timestamp;
/* always set heartbeat, publish only if telemetry link not up */
tstatus.heartbeat_time = tnow;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
if (!_radio_status_available) {
tstatus.timestamp = tnow;
/* telem_time indicates the timestamp of a telemetry status packet and we got none */
tstatus.telem_time = 0;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
}
}
src/modules/mavlink/mavlink_receiver.h
+1 -1
View File
@@ -110,6 +110,7 @@ private:
void handle_message(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_optical_flow(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
@@ -151,7 +152,6 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
bool _radio_status_available;
int _control_mode_sub;
int _hil_frames;
uint64_t _old_timestamp;