QuRT and POSIX changes - part 3

More staged changes to support QuRT and related POSIX changes

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

src/modules/commander/accelerometer_calibration.cpp

@@ -132,7 +132,7 @@
 #include <stdio.h>
 #include <poll.h>
 #include <fcntl.h>
-#include <sys/prctl.h>
+//#include <sys/prctl.h>
 #include <math.h>
 #include <float.h>
 #include <mathlib/mathlib.h>

src/modules/commander/airspeed_calibration.cpp

@@ -41,11 +41,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 <drivers/drv_hrt.h>
 #include <drivers/drv_airspeed.h>
 #include <uORB/topics/sensor_combined.h>
@@ -157,7 +158,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 	diff_pres_offset = diff_pres_offset / calibration_count;
 
-	if (isfinite(diff_pres_offset)) {
+	if (PX4_ISFINITE(diff_pres_offset)) {
 
 		int  fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
 		airscale.offset_pa = diff_pres_offset;

src/modules/mavlink/mavlink_main_posix.cpp

@@ -51,7 +51,9 @@
 #include <assert.h>
 #include <math.h>
 #include <poll.h>
+#ifndef __PX4_QURT
 #include <termios.h>
+#endif
 #include <time.h>
 #include <math.h> /* isinf / isnan checks */
 
@@ -138,7 +140,9 @@ Mavlink::Mavlink() :
 	_forwarding_on(false),
 	_passing_on(false),
 	_ftp_on(false),
+#ifndef __PX4_QURT
 	_uart_fd(-1),
+#endif
 	_baudrate(57600),
 	_datarate(1000),
 	_datarate_events(500),
@@ -395,6 +399,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 	}
 }
 
+#ifndef __PX4_QURT
 int
 Mavlink::get_uart_fd(unsigned index)
 {
@@ -412,6 +417,7 @@ Mavlink::get_uart_fd()
 {
 	return _uart_fd;
 }
+#endif // __PX4_QURT
 
 int
 Mavlink::get_instance_id()
@@ -546,6 +552,7 @@ int Mavlink::get_component_id()
 	return mavlink_system.compid;
 }
 
+#ifndef __PX4_QURT
 int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
 {
 	/* process baud rate */
@@ -697,6 +704,8 @@ Mavlink::enable_flow_control(bool enabled)
 	return ret;
 }
 
+#endif
+
 int
 Mavlink::set_hil_enabled(bool hil_enabled)
 {
@@ -729,8 +738,10 @@ Mavlink::get_free_tx_buf()
 	 */
 	int buf_free = 0;
 
+#ifndef __PX4_QURT
+
 // No FIONWRITE on Linux
-#ifndef __PX4_LINUX
+#if !defined(__PX4_LINUX)
 	(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
 #endif
 
@@ -746,6 +757,7 @@ Mavlink::get_free_tx_buf()
 			enable_flow_control(false);
 		}
 	}
+#endif
 
 	return buf_free;
 }
@@ -800,6 +812,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
 
+#ifndef __PX4_QURT
 	/* send message to UART */
 	ssize_t ret = ::write(_uart_fd, buf, packet_len);
 
@@ -811,6 +824,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 		_last_write_success_time = _last_write_try_time;
 		count_txbytes(packet_len);
 	}
+#endif
 
 	pthread_mutex_unlock(&_send_mutex);
 }
@@ -850,6 +864,7 @@ Mavlink::resend_message(mavlink_message_t *msg)
 	buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
 	buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
 
+#ifndef __PX4_QURT
 	/* send message to UART */
 	ssize_t ret = ::write(_uart_fd, buf, packet_len);
 
@@ -861,6 +876,7 @@ Mavlink::resend_message(mavlink_message_t *msg)
 		_last_write_success_time = _last_write_try_time;
 		count_txbytes(packet_len);
 	}
+#endif
 
 	pthread_mutex_unlock(&_send_mutex);
 }
@@ -1299,6 +1315,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
 
+#ifndef __PX4_QURT
 	struct termios uart_config_original;
 
 	/* default values for arguments */
@@ -1308,6 +1325,7 @@ Mavlink::task_main(int argc, char *argv[])
 		warn("could not open %s", _device_name);
 		return ERROR;
 	}
+#endif
 
 	/* initialize send mutex */
 	pthread_mutex_init(&_send_mutex, NULL);
@@ -1560,11 +1578,13 @@ Mavlink::task_main(int argc, char *argv[])
 	/* wait for threads to complete */
 	pthread_join(_receive_thread, NULL);
 
+#ifndef __PX4_QURT
 	/* reset the UART flags to original state */
 	tcsetattr(_uart_fd, TCSANOW, &uart_config_original);
 
 	/* close UART */
 	::close(_uart_fd);
+#endif
 
 	/* close mavlink logging device */
 	px4_close(_mavlink_fd);

src/platforms/px4_defines.h

@@ -106,7 +106,7 @@ typedef param_t px4_param_t;
 
 #define PX4_ISFINITE(x) isfinite(x)
 
-/* Linux Specific defines */
+/* POSIX Specific defines */
 #elif defined(__PX4_POSIX)
 
 // Flag is meaningless on Linux
@@ -138,7 +138,16 @@ __END_DECLS
 #define OK 0
 #define ERROR -1
 
+#if defined(__PX4_QURT)
+#define M_PI			3.14159265358979323846
+#define M_PI_2			1.57079632679489661923
+__BEGIN_DECLS
 #include <math.h>
+__END_DECLS
+#else
+#include <math.h>
+#endif
+
 /* Float defines of the standard double length constants  */
 #define M_E_F			(float)M_E
 #define M_LOG2E_F		(float)M_LOG2E
@@ -167,7 +176,30 @@ __END_DECLS
 #define M_DEG_TO_RAD 		0.01745329251994
 #define M_RAD_TO_DEG 		57.2957795130823
 
+#ifndef __PX4_QURT
+
+#if defined(__cplusplus)
 #define PX4_ISFINITE(x) std::isfinite(x)
+#else
+#define PX4_ISFINITE(x) isfinite(x)
+#endif
+#endif
+
+#endif
+
+#if defined(__PX4_QURT)
+#define SIOCDEVPRIVATE 999999
+
+// Missing math.h defines
+#define PX4_ISFINITE(x) isfinite(x)
+
+// FIXME - these are missing for clang++ but not for clang
+#if defined(__cplusplus)
+#define isfinite(x) true
+#define isnan(x) false
+#define isinf(x) false
+#define fminf(x, y) ((x) > (y) ? y : x)
+#endif
 
 #endif