QuRT and POSIX changes - part 5

Last part of the main QuRT related changes

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

src/modules/commander/gyro_calibration.cpp

@@ -42,11 +42,12 @@
 #include "commander_helper.h"
 
 #include <px4_posix.h>
+#include <px4_time.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <fcntl.h>
 #include <poll.h>
-#include <math.h>
+#include <cmath>
 #include <string.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/sensor_combined.h>
@@ -193,9 +194,9 @@ int do_gyro_calibration(int mavlink_fd)
 		/* maximum allowable calibration error in radians */
 		const float maxoff = 0.0055f;
 
-		if (!isfinite(gyro_scale[0].x_offset) ||
-		    !isfinite(gyro_scale[0].y_offset) ||
-		    !isfinite(gyro_scale[0].z_offset) ||
+		if (!PX4_ISFINITE(gyro_scale[0].x_offset) ||
+		    !PX4_ISFINITE(gyro_scale[0].y_offset) ||
+		    !PX4_ISFINITE(gyro_scale[0].z_offset) ||
 		    fabsf(xdiff) > maxoff ||
 		    fabsf(ydiff) > maxoff ||
 		    fabsf(zdiff) > maxoff) {

src/modules/commander/mag_calibration.cpp

@@ -43,12 +43,13 @@
 #include "calibration_messages.h"
 
 #include <px4_posix.h>
+#include <px4_time.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <stdlib.h>
 #include <string.h>
 #include <poll.h>
-#include <math.h>
+#include <cmath>
 #include <fcntl.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
@@ -376,7 +377,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 							 &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
 							 &sphere_radius[cur_mag]);
 				
-				if (!isfinite(sphere_x[cur_mag]) || !isfinite(sphere_y[cur_mag]) || !isfinite(sphere_z[cur_mag])) {
+				if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
 					mavlink_and_console_log_info(mavlink_fd, "ERROR: NaN in sphere fit for mag #%u", cur_mag);
 					result = ERROR;
 				}

src/modules/commander/rc_calibration.cpp

@@ -36,6 +36,9 @@
  * Remote Control calibration routine
  */
 
+#include <px4_posix.h>
+#include <px4_time.h>
+
 #include "rc_calibration.h"
 #include "commander_helper.h"
 
@@ -82,11 +85,11 @@ int do_trim_calibration(int mavlink_fd)
 
 	if (save_ret != 0) {
 		mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
-		close(sub_man);
+		px4_close(sub_man);
 		return ERROR;
 	}
 
 	mavlink_log_info(mavlink_fd, "trim cal done");
-	close(sub_man);
+	px4_close(sub_man);
 	return OK;
 }

src/modules/mavlink/mavlink_main.h

@@ -112,9 +112,11 @@ public:
 
 	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
 
+#ifndef __PX4_QURT
 	static int		get_uart_fd(unsigned index);
 
 	int			get_uart_fd();
+#endif
 
 	/**
 	 * Get the MAVLink system id.
@@ -190,12 +192,14 @@ public:
 
 	int			get_instance_id();
 
+#ifndef __PX4_QURT
 	/**
 	 * Enable / disable hardware flow control.
 	 *
 	 * @param enabled	True if hardware flow control should be enabled
 	 */
 	int			enable_flow_control(bool enabled);
+#endif
 
 	mavlink_channel_t	get_channel();
 
@@ -320,7 +324,9 @@ private:
 	bool			_forwarding_on;
 	bool			_passing_on;
 	bool			_ftp_on;
+#ifndef __PX4_QURT
 	int			_uart_fd;
+#endif
 	int			_baudrate;
 	int			_datarate;		///< data rate for normal streams (attitude, position, etc.)
 	int			_datarate_events;	///< data rate for params, waypoints, text messages
@@ -378,7 +384,9 @@ private:
 
 	void			mavlink_update_system();
 
+#ifndef __PX4_QURT
 	int			mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
+#endif
 
 	static unsigned int	interval_from_rate(float rate);
 

src/modules/mavlink/mavlink_messages.cpp

@@ -39,6 +39,7 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
+#include <px4_time.h>
 #include <stdio.h>
 
 #include <commander/px4_custom_mode.h>
@@ -354,6 +355,7 @@ protected:
 		}
 	}
 
+#ifndef __PX4_QURT
 	void send(const hrt_abstime t)
 	{
 		if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) {
@@ -391,7 +393,7 @@ protected:
 						char log_file_path[64] = "";
 
 						timespec ts;
-						clock_gettime(CLOCK_REALTIME, &ts);
+						px4_clock_gettime(CLOCK_REALTIME, &ts);
 						/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
 						time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
 						struct tm tt;
@@ -410,6 +412,7 @@ protected:
 			}
 		}
 	}
+#endif
 };
 
 class MavlinkStreamCommandLong : public MavlinkStream
@@ -975,7 +978,7 @@ protected:
 		mavlink_system_time_t msg;
 		timespec tv;
 
-		clock_gettime(CLOCK_REALTIME, &tv);
+		px4_clock_gettime(CLOCK_REALTIME, &tv);
 
 		msg.time_boot_ms = hrt_absolute_time() / 1000;
 		msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000;

src/modules/mavlink/mavlink_receiver.cpp

@@ -41,12 +41,8 @@
  */
 
 /* XXX trim includes */
-#ifdef __PX4_NUTTX
-#include <nuttx/config.h>
-#include <nuttx/sched.h>
-#else
 #include <px4_config.h>
-#endif
+#include <px4_time.h>
 #include <unistd.h>
 #include <pthread.h>
 #include <stdio.h>
@@ -64,8 +60,10 @@
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
+#ifndef __PX4_QURT
 #include <sys/prctl.h>
 #include <termios.h>
+#endif
 #include <errno.h>
 #include <stdlib.h>
 #include <poll.h>
@@ -993,7 +991,7 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
 	mavlink_msg_system_time_decode(msg, &time);
 
 	timespec tv;
-	clock_gettime(CLOCK_REALTIME, &tv);
+	px4_clock_gettime(CLOCK_REALTIME, &tv);
 
 	// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
 	bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS;
@@ -1002,7 +1000,7 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
 	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)) {
+		if(px4_clock_settime(CLOCK_REALTIME, &tv)) {
 			warn("failed setting clock");
 		}
 		else {
@@ -1488,6 +1486,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 void *
 MavlinkReceiver::receive_thread(void *arg)
 {
+#ifndef __PX4_QURT
 	int uart_fd = _mavlink->get_uart_fd();
 
 	const int timeout = 500;
@@ -1530,6 +1529,7 @@ MavlinkReceiver::receive_thread(void *arg)
 			_mavlink->count_rxbytes(nread);
 		}
 	}
+#endif
 
 	return NULL;
 }
@@ -1576,9 +1576,11 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
 
+#ifndef __PX4_QURT
 	// set to non-blocking read
 	int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0);
 	fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);
+#endif
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&receiveloop_attr, &param);

src/modules/mavlink/mavlink_stream.cpp

@@ -79,7 +79,9 @@ MavlinkStream::update(const hrt_abstime t)
 
 	if (dt > 0 && dt >= interval) {
 		/* interval expired, send message */
+#ifndef __PX4_QURT
 		send(t);
+#endif
 
 		if (const_rate()) {
 			_last_sent = (t / _interval) * _interval;

src/modules/mavlink/mavlink_stream.h

@@ -92,7 +92,9 @@ protected:
 	Mavlink     *_mavlink;
 	unsigned int _interval;
 
+#ifndef __PX4_QURT
 	virtual void send(const hrt_abstime t) = 0;
+#endif
 
 private:
 	hrt_abstime _last_sent;

src/modules/systemlib/mavlink_log.c

@@ -39,6 +39,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include <px4_posix.h>
 #include <string.h>
 #include <stdlib.h>
 #include <stdio.h>
@@ -124,5 +125,5 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...)
 	char text[MAVLINK_LOG_MAXLEN + 1];
 	vsnprintf(text, sizeof(text), fmt, ap);
 	va_end(ap);
-	ioctl(_fd, severity, (unsigned long)&text[0]);
+	px4_ioctl(_fd, severity, (unsigned long)&text[0]);
 }

src/platforms/px4_time.h

@@ -3,7 +3,7 @@
 #include <sys/types.h>
 #include <time.h>
 
-#ifdef __PX4_LINUX
+#if defined(__PX4_LINUX) || defined(__PX4_NUTTX)
 
 #define px4_clock_gettime clock_gettime
 #define px4_clock_settime clock_settime