use gcc attributes to align and pack

msg/templates/uorb/msg.h.template

@@ -173,7 +173,8 @@ def print_parsed_fields():
 
 }@
 
-// #pragma pack(push, 1) badly breaks alignment everywhere!
+// #pragma pack(push, 1)
+ORBPACKED(
 #ifdef __cplusplus
 @#class @(uorb_struct) {
 struct __EXPORT @(uorb_struct) {
@@ -198,7 +199,7 @@ for constant in spec.constants:
   print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
 }
 #endif
-};
+});
 //#pragma pack(pop)
 
 /* register this as object request broker structure */

src/drivers/device/integrator.cpp

@@ -60,7 +60,7 @@ Integrator::~Integrator()
 }
 
 bool
-Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt)
+Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t *integral_dt)
 {
 	if (_last_integration_time == 0) {
 		/* this is the first item in the integrator */
@@ -132,7 +132,7 @@ Integrator::put_with_interval(unsigned interval_us, math::Vector<3> &val, math::
 }
 
 math::Vector<3>
-Integrator::get(bool reset, uint64_t &integral_dt)
+Integrator::get(bool reset, uint64_t *integral_dt)
 {
 	math::Vector<3> val = _integral;
 
@@ -158,12 +158,12 @@ Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3>
 }
 
 void
-Integrator::_reset(uint64_t &integral_dt)
+Integrator::_reset(uint64_t *integral_dt)
 {
 	_integral(0) = 0.0f;
 	_integral(1) = 0.0f;
 	_integral(2) = 0.0f;
 
-	integral_dt = (_last_integration_time - _last_reset_time);
+	*integral_dt = (_last_integration_time - _last_reset_time);
 	_last_reset_time = _last_integration_time;
 }

src/drivers/device/integrator.h

@@ -60,7 +60,7 @@ public:
 	 * @return		true if putting the item triggered an integral reset and the integral should be
 	 *			published.
 	 */
-	bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt);
+	bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t *integral_dt);
 
 	/**
 	 * Put an item into the integral but provide an interval instead of a timestamp.
@@ -84,7 +84,7 @@ public:
 	 * @param integral_dt	Get the dt in us of the current integration (only if reset).
 	 * @return		the integral since the last read-reset
 	 */
-	math::Vector<3>		get(bool reset, uint64_t &integral_dt);
+	math::Vector<3>		get(bool reset, uint64_t *integral_dt);
 
 	/**
 	 * Get the current integral and reset the integrator if needed. Additionally give the
@@ -115,5 +115,5 @@ private:
 	 *
 	 * @param integral_dt	Get the dt in us of the current integration.
 	 */
-	void _reset(uint64_t &integral_dt);
+	void _reset(uint64_t *integral_dt);
 };

src/drivers/l3gd20/l3gd20.cpp

@@ -1082,7 +1082,7 @@ L3GD20::measure()
 	math::Vector<3> gval(xin, yin, zin);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt);
+	bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, &report.integral_dt);
 	report.x_integral = gval_integrated(0);
 	report.y_integral = gval_integrated(1);
 	report.z_integral = gval_integrated(2);

src/drivers/lsm303d/lsm303d.cpp

@@ -1649,7 +1649,7 @@ LSM303D::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
+	bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, &accel_report.integral_dt);
 	accel_report.x_integral = aval_integrated(0);
 	accel_report.y_integral = aval_integrated(1);
 	accel_report.z_integral = aval_integrated(2);

src/drivers/mpu6000/mpu6000.cpp

@@ -1905,7 +1905,7 @@ MPU6000::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1945,7 +1945,7 @@ MPU6000::measure()
 	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, &grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);

src/drivers/mpu6500/mpu6500.cpp

@@ -1846,7 +1846,7 @@ MPU6500::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1881,7 +1881,7 @@ MPU6500::measure()
 	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, &grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);

src/drivers/mpu9250/mpu9250.cpp

@@ -1418,7 +1418,7 @@ MPU9250::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1453,7 +1453,7 @@ MPU9250::measure()
 	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, &grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);

src/drivers/px4flow/px4flow.cpp

@@ -541,9 +541,9 @@ PX4FLOW::collect()
 	/* rotate measurements according to parameter */
 	float zeroval = 0.0f;
 
-	rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
+	rotate_3f(_sensor_rotation, &report.pixel_flow_x_integral, &report.pixel_flow_y_integral, &zeroval);
 
-	rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
+	rotate_3f(_sensor_rotation, &report.gyro_x_rate_integral, &report.gyro_y_rate_integral, &report.gyro_z_rate_integral);
 
 	if (_px4flow_topic == nullptr) {
 		_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);

src/lib/conversion/rotation.cpp

@@ -52,6 +52,16 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix)
 
 #define HALF_SQRT_2 0.70710678118654757f
 
+__EXPORT void
+rotate_3f(enum Rotation rot, float *x, float *y, float *z)
+{
+	float &x2 = *x;
+	float &y2 = *y;
+	float &z2 = *z;
+
+	rotate_3f(rot, x2, y2, z2);
+}
+
 __EXPORT void
 rotate_3f(enum Rotation rot, float &x, float &y, float &z)
 {

src/lib/conversion/rotation.h

@@ -130,6 +130,11 @@ const rot_lookup_t rot_lookup[] = {
 __EXPORT void
 get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix);
 
+/**
+ * rotate a 3 element float vector in-place
+ */
+__EXPORT void
+rotate_3f(enum Rotation rot, float *x, float *y, float *z);
 
 /**
  * rotate a 3 element float vector in-place

src/modules/mavlink/mavlink_receiver.cpp

@@ -490,8 +490,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 	enum Rotation flow_rot;
 	param_get(param_find("SENS_FLOW_ROT"), &flow_rot);
 
-	struct optical_flow_s f;
-	memset(&f, 0, sizeof(f));
+	struct optical_flow_s f = {};
 
 	f.timestamp = flow.time_usec;
 	f.integration_timespan = flow.integration_time_us;
@@ -508,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,6 +46,13 @@
 // 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
+
 /**
  * Object metadata.
  */

src/platforms/posix/drivers/gyrosim/gyrosim.cpp

@@ -1103,7 +1103,7 @@ GYROSIM::_measure()
 	math::Vector<3> aval(mpu_report.accel_x, mpu_report.accel_y, mpu_report.accel_z);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1125,7 +1125,7 @@ GYROSIM::_measure()
 	math::Vector<3> gval(mpu_report.gyro_x, mpu_report.gyro_y, mpu_report.gyro_z);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, &grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);