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

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

Browse Source 
Conflicts:
	src/lib/mathlib/math/Matrix.hpp
	src/modules/mc_att_control/mc_att_control_main.cpp
	src/modules/uORB/topics/vehicle_status.h
	src/platforms/px4_includes.h
This commit is contained in:
Thomas Gubler
2015-01-05 10:02:07 +01:00
parent e16c4ff76e c3ed35f5cc
commit 16b9f666e7
83 changed files with 3468 additions and 1194 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+106 -10
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -68,6 +68,8 @@
#include <mathlib/mathlib.h>
#include <conversion/rotation.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
@@ -121,7 +123,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_old_timestamp(0),
_hil_local_proj_inited(0),
_hil_local_alt0(0.0f),
_hil_local_proj_ref{}
_hil_local_proj_ref{},
_time_offset_avg_alpha(0.6),
_time_offset(0)
{
// make sure the FTP server is started
@@ -188,6 +192,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
MavlinkFTP::get_server()->handle_message(_mavlink, msg);
break;
case MAVLINK_MSG_ID_SYSTEM_TIME:
handle_message_system_time(msg);
break;
case MAVLINK_MSG_ID_TIMESYNC:
handle_message_timesync(msg);
break;
default:
break;
}
@@ -251,7 +263,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
//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");
warnx("terminated by remote");
fflush(stdout);
usleep(50000);
@@ -261,7 +273,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
warnx("ignoring CMD with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
return;
}
@@ -307,7 +319,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
//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");
warnx("terminated by remote");
fflush(stdout);
usleep(50000);
@@ -317,7 +329,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
warnx("ignoring CMD with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
return;
}
@@ -358,6 +370,9 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
mavlink_optical_flow_rad_t flow;
mavlink_msg_optical_flow_rad_decode(msg, &flow);
enum Rotation flow_rot;
param_get(param_find("SENS_FLOW_ROT"),&flow_rot);
struct optical_flow_s f;
memset(&f, 0, sizeof(f));
@@ -374,6 +389,10 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
f.sensor_id = flow.sensor_id;
f.gyro_temperature = flow.temperature;
/* rotate measurements according to parameter */
float zeroval = 0.0f;
rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval);
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -689,7 +708,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
// Use the component ID to identify the vision sensor
vision_position.id = msg->compid;
vision_position.timestamp_boot = hrt_absolute_time();
vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time
vision_position.timestamp_computer = pos.usec;
vision_position.x = pos.x;
vision_position.y = pos.y;
@@ -861,7 +880,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f;
warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
// warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
@@ -918,6 +937,66 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
{
mavlink_system_time_t time;
mavlink_msg_system_time_decode(msg, &time);
timespec tv;
clock_gettime(CLOCK_REALTIME, &tv);
// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS;
bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL;
if (!onb_unix_valid && ofb_unix_valid) {
tv.tv_sec = time.time_unix_usec / 1000000ULL;
tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL;
if(clock_settime(CLOCK_REALTIME, &tv)) {
warn("failed setting clock");
}
else {
warnx("[timesync] UTC time synced.");
}
}
}
void
MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
{
mavlink_timesync_t tsync;
mavlink_msg_timesync_decode(msg, &tsync);
uint64_t now_ns = hrt_absolute_time() * 1000LL ;
if (tsync.tc1 == 0) {
mavlink_timesync_t rsync; // return timestamped sync message
rsync.tc1 = now_ns;
rsync.ts1 = tsync.ts1;
_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync);
return;
} else if (tsync.tc1 > 0) {
int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ;
int64_t dt = _time_offset - offset_ns;
if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew
_time_offset = offset_ns;
warnx("[timesync] Hard setting offset.");
} else {
smooth_time_offset(offset_ns);
}
}
}
void
MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
{
@@ -1110,7 +1189,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* print HIL sensors rate */
if ((timestamp - _old_timestamp) > 10000000) {
printf("receiving HIL sensors at %d hz\n", _hil_frames / 10);
// printf("receiving HIL sensors at %d hz\n", _hil_frames / 10);
_old_timestamp = timestamp;
_hil_frames = 0;
}
@@ -1128,7 +1207,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
memset(&hil_gps, 0, sizeof(hil_gps));
hil_gps.timestamp_time = timestamp;
hil_gps.time_gps_usec = gps.time_usec;
hil_gps.time_utc_usec = gps.time_usec;
hil_gps.timestamp_position = timestamp;
hil_gps.lat = gps.lat;
@@ -1385,6 +1464,23 @@ void MavlinkReceiver::print_status()
}
uint64_t MavlinkReceiver::to_hrt(uint64_t usec)
{
return usec - (_time_offset / 1000) ;
}
void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns)
{
/* alpha = 0.6 fixed for now. The closer alpha is to 1.0,
* the faster the moving average updates in response to
* new offset samples.
*/
_time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset;
}
void *MavlinkReceiver::start_helper(void *context)
{
MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context);