Nuttx: fixups after rebase on Linux

Seems that mavlink_receiver_linux.cpp inherited the history
from mavlink_receiver.cpp so updates went to it vs mavlink_receiver_nuttx.cpp

Two module.mk files used ifdef instead of ifeq.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/drivers/drv_led.h

@@ -39,6 +39,7 @@
 
 #pragma once
 
+#include <px4_defines.h>
 #include <stdint.h>
 #include <sys/ioctl.h>
 

src/drivers/hil/module.mk

@@ -37,7 +37,7 @@
 
 MODULE_COMMAND	= hil
 
-ifdef ($(PX4_TARGET_OS),nuttx)
+ifeq ($(PX4_TARGET_OS),nuttx)
 SRCS		= hil.cpp
 MAXOPTIMIZATION	 = -Os
 else

src/modules/commander/calibration_routines.cpp

@@ -38,6 +38,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include <px4_defines.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <math.h>
@@ -53,9 +54,6 @@
 #include "calibration_messages.h"
 #include "commander_helper.h"
 
-// FIXME: Fix return codes
-//static const int ERROR = -1;
-
 int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
 			     float *sphere_radius)
@@ -414,7 +412,7 @@ int calibrate_from_orientation(int	mavlink_fd,
 	int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
 	if (sub_accel < 0) {
 		mavlink_and_console_log_critical(mavlink_fd, "No onboard accel found, abort");
-		return ERROR;
+		return PX4_ERROR;
 	}
 	
 	unsigned orientation_failures = 0;
@@ -422,7 +420,7 @@ int calibrate_from_orientation(int	mavlink_fd,
 	// Rotate through all three main positions
 	while (true) {
 		if (orientation_failures > 10) {
-			result = ERROR;
+			result = PX4_ERROR;
 			mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_ORIENTATION_TIMEOUT);
 			break;
 		}

src/modules/commander/module.mk

@@ -59,7 +59,7 @@ MODULE_STACKSIZE = 5000
 
 MAXOPTIMIZATION	 = -Os
 
-ifdef ($(PX4_TARGET_OS),nuttx)
+ifeq ($(PX4_TARGET_OS),nuttx)
 EXTRACXXFLAGS = -Wframe-larger-than=2200
 endif
 

src/modules/mavlink/mavlink_receiver_nuttx.cpp

@@ -121,6 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_rc_pub(-1),
 	_manual_pub(-1),
 	_land_detector_pub(-1),
+	_time_offset_pub(-1),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
@@ -157,6 +158,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_optical_flow_rad(msg);
 		break;
 
+	case MAVLINK_MSG_ID_PING:
+		handle_message_ping(msg);
+		break;
+
 	case MAVLINK_MSG_ID_SET_MODE:
 		handle_message_set_mode(msg);
 		break;
@@ -725,8 +730,8 @@ 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 = to_hrt(pos.usec); // Synced time
-	vision_position.timestamp_computer = pos.usec;
+	vision_position.timestamp_boot = hrt_absolute_time(); // Monotonic time
+	vision_position.timestamp_computer = sync_stamp(pos.usec); // Synced time
 	vision_position.x = pos.x;
 	vision_position.y = pos.y;
 	vision_position.z = pos.z;
@@ -946,6 +951,19 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
+{
+	mavlink_ping_t ping;
+	mavlink_msg_ping_decode( msg, &ping);
+	if ((mavlink_system.sysid == ping.target_system) &&
+		(mavlink_system.compid == ping.target_component)) {
+		mavlink_message_t msg_out;
+		mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping);
+		_mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out);
+	}
+}
+
 void
 MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
 {
@@ -996,6 +1014,9 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
 	mavlink_timesync_t tsync;
 	mavlink_msg_timesync_decode(msg, &tsync);
 
+	struct time_offset_s tsync_offset;
+	memset(&tsync_offset, 0, sizeof(tsync_offset));
+
 	uint64_t now_ns = hrt_absolute_time() * 1000LL ;
 
 	if (tsync.tc1 == 0) {
@@ -1022,6 +1043,15 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
 		}
 	}
 
+	tsync_offset.offset_ns = _time_offset ;
+
+	if (_time_offset_pub < 0) {
+		_time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset);
+
+	} else {
+		orb_publish(ORB_ID(time_offset), _time_offset_pub, &tsync_offset);
+	}
+
 }
 
 void
@@ -1505,9 +1535,12 @@ void MavlinkReceiver::print_status()
 
 }
 
-uint64_t MavlinkReceiver::to_hrt(uint64_t usec)
+uint64_t MavlinkReceiver::sync_stamp(uint64_t usec)
 {
-	return usec - (_time_offset / 1000) ;
+	if(_time_offset > 0)
+		return usec - (_time_offset / 1000) ;
+	else
+		return hrt_absolute_time();
 }