WIP logger serialization

src/modules/commander/commander.cpp

@@ -2295,9 +2295,8 @@ int commander_thread_main(int argc, char *argv[])
 		 * rejection. Back off 2 seconds to not overlay
 		 * home tune.
 		 */
-		uint64_t ts = _home.timestamp;
 		if (status_flags.condition_home_position_valid &&
-			(hrt_elapsed_time(&ts) > 2000000) &&
+			(hrt_elapsed_time(&_home.timestamp) > 2000000) &&
 			_last_mission_instance != mission_result.instance_count) {
 			if (!mission_result.valid) {
 				/* the mission is invalid */

src/modules/logger/logger.cpp

@@ -4,8 +4,11 @@
 #include <errno.h>
 #include <string.h>
 
+#include <uORB/uORB.h>
 #include <uORB/uORBTopics.h>
+#include <px4_includes.h>
 #include <px4_getopt.h>
+#include <px4_log.h>
 
 #define DBGPRINT
 
@@ -25,6 +28,14 @@ using namespace px4::logger;
 
 int logger_main(int argc, char *argv[])
 {
+	// logger currently assumes little endian
+	int num = 1;
+
+	if (*(char *)&num != 1) {
+		PX4_ERR("Logger only works on little endian!\n");
+		return 1;
+	}
+
 	if (argc < 2) {
 		PX4_INFO("usage: logger {start|stop|status}");
 		return 1;
@@ -196,7 +207,12 @@ void Logger::run_trampoline(int argc, char *argv[])
 	warnx("remaining free heap: %d bytes", alloc_info.fordblks);
 #endif /* DBGPRINT */
 
-	logger_ptr->run();
+	if (logger_ptr == nullptr) {
+		PX4_WARN("alloc failed");
+
+	} else {
+		logger_ptr->run();
+	}
 }
 
 enum class MessageType : uint8_t {
@@ -480,8 +496,8 @@ void Logger::run()
 				/* each message consists of a header followed by an orb data object
 				 * The size of the data object is given by orb_metadata.o_size
 				 */
-				size_t msg_size = sizeof(message_data_header_s) + sub.metadata->o_size;
-				uint8_t buffer[msg_size];
+				size_t padded_msg_size = sub.metadata->o_size;
+				uint8_t orb_msg_padded[padded_msg_size];
 
 				/* if this topic has been updated, copy the new data into the message buffer
 				 * and write a message to the log
@@ -489,8 +505,15 @@ void Logger::run()
 				//orb_check(sub.fd, &updated);	// check whether a non-multi topic has been updated
 				/* this works for both single and multi-instances */
 				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
-					if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], buffer + sizeof(message_data_header_s),
-								  &sub.time_tried_subscribe)) {
+					if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], &orb_msg_padded, &sub.time_tried_subscribe)) {
+
+						struct orb_output_buffer output_buffer = {};
+						uint8_t buffer[sizeof(message_data_header_s) + padded_msg_size];
+						output_buffer.data = &buffer;
+						output_buffer.next = sizeof(message_data_header_s);
+
+						sub.metadata->serialize(&orb_msg_padded, &output_buffer);
+						size_t msg_size = output_buffer.next;
 
 						//uint64_t timestamp;
 						//memcpy(&timestamp, buffer + sizeof(message_data_header_s), sizeof(timestamp));
@@ -505,10 +528,14 @@ void Logger::run()
 						 */
 						message_data_header_s *header = reinterpret_cast<message_data_header_s *>(buffer);
 						header->msg_type = static_cast<uint8_t>(MessageType::DATA);
-						header->msg_size = static_cast<uint16_t>(msg_size - 3);
+						/* the ORB topic data object has 2 unused trailing bytes? */
+						//header->msg_size = static_cast<uint16_t>(msg_size - 3);
+						header->msg_size = msg_size;
 						header->msg_id = msg_id;
 						header->multi_id = 0x80 + instance;	// Non multi, active
 
+						//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);
+
 #ifdef DBGPRINT
 						//warnx("subscription %s updated: %d, size: %d", sub.metadata->o_name, updated, msg_size);
 						hrt_abstime trytime = hrt_absolute_time();
@@ -527,7 +554,9 @@ void Logger::run()
 							if (dropout_start != 0) {
 								double drop_len = (double)(trytime - dropout_start)  * 1e-6;
 
-								if (drop_len > max_drop_len) { max_drop_len = drop_len; }
+								if (drop_len > max_drop_len) {
+									max_drop_len = drop_len;
+								}
 
 								PX4_WARN("dropout length: %5.3f seconds", drop_len);
 								dropout_start = 0;

src/modules/logger/logger.h

@@ -32,7 +32,9 @@ struct LoggerSubscription {
 		fd[0] = fd_;
 		time_tried_subscribe = 0;
 
-		for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) { fd[i] = -1; }
+		for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			fd[i] = -1;
+		}
 	}
 };
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -507,8 +507,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 
 	/* rotate measurements according to parameter */
 	float zeroval = 0.0f;
-	rotate_3f(flow_rot, &f.pixel_flow_x_integral, &f.pixel_flow_y_integral, &zeroval);
-	rotate_3f(flow_rot, &f.gyro_x_rate_integral, &f.gyro_y_rate_integral, &f.gyro_z_rate_integral);
+	rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval);
+	rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral);
 
 	if (_flow_pub == nullptr) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);

src/modules/uORB/uORB.h

@@ -46,12 +46,12 @@
 // Hack until everything is using this header
 #include <systemlib/visibility.h>
 
-// Macro to define packed structures
-#ifdef __GNUC__
-#define ORBPACKED( __Declaration__ ) __Declaration__ __attribute__((aligned(4), packed))
-#else
-#define ORBPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) )
-#endif
+struct orb_output_buffer {
+	void *data;
+	size_t next;
+};
+
+typedef void (*func_ptr)(void *in, struct orb_output_buffer *out);
 
 /**
  * Object metadata.
@@ -59,6 +59,7 @@
 struct orb_metadata {
 	const char *o_name;		/**< unique object name */
 	const size_t o_size;		/**< object size */
+	func_ptr serialize;			/**< serialization function for this orb topic */
 	const char *o_fields;		/**< semicolon separated list of fields */
 };
 
@@ -118,13 +119,15 @@ enum ORB_PRIO {
  *
  * @param _name		The name of the topic.
  * @param _struct	The structure the topic provides.
- * @param _func		The pointer to a function that packs topic
+ * @param _func		The pointer to a function that serializes this topic
+ * @param _fields	All fields in a semicolon separated list e.g: "float[3] position;bool armed"
  */
-#define ORB_DEFINE(_name, _struct, _fields)			\
+#define ORB_DEFINE(_name, _struct, _func, _fields)			\
 	const struct orb_metadata __orb_##_name = {	\
 		#_name,					\
 		sizeof(_struct),		\
-		_fields				\
+		_func,					\
+		_fields					\
 	}; struct hack
 
 __BEGIN_DECLS

src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp

@@ -41,8 +41,8 @@ struct orb_test {
 	int val;
 	hrt_abstime time;
 };
-ORB_DEFINE(orb_test, struct orb_test, "ORB_TEST:int val;hrt_abstime time;");
-ORB_DEFINE(orb_multitest, struct orb_test, "ORB_MULTITEST:int val;hrt_abstime time;");
+ORB_DEFINE(orb_test, struct orb_test, nullptr, "ORB_TEST:int val;hrt_abstime time;");
+ORB_DEFINE(orb_multitest, struct orb_test, nullptr, "ORB_MULTITEST:int val;hrt_abstime time;");
 
 
 struct orb_test_medium {
@@ -50,8 +50,8 @@ struct orb_test_medium {
 	hrt_abstime time;
 	char junk[64];
 };
-ORB_DEFINE(orb_test_medium, struct orb_test_medium,  "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
-ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium,
+ORB_DEFINE(orb_test_medium, struct orb_test_medium, nullptr, "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
+ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, nullptr,
 	   "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
 
 
@@ -60,7 +60,7 @@ struct orb_test_large {
 	hrt_abstime time;
 	char junk[512];
 };
-ORB_DEFINE(orb_test_large, struct orb_test_large, "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
+ORB_DEFINE(orb_test_large, struct orb_test_large, nullptr, "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
 
 
 namespace uORBTest