aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink/mavlink_receiver.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp116
1 files changed, 106 insertions, 10 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index cb55a25aa..ce3f2ae0e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -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);
@@ -919,6 +938,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)
{
mavlink_hil_sensor_t imu;
@@ -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);
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-02 19:37:07 +0000