diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template index 7fe48e593d..564657c2e9 100644 --- a/msg/templates/uorb/msg.h.template +++ b/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 */ diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp index 072e3096a2..86a9b53505 100644 --- a/src/drivers/device/integrator.cpp +++ b/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; } diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h index 48a232066f..86817323e2 100644 --- a/src/drivers/device/integrator.h +++ b/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); }; diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 61345628b1..e0b2c39c80 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/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); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 71796cfe8c..6563282f4e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/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); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 4bf6384b3f..a6b2b2e6f1 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/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); diff --git a/src/drivers/mpu6500/mpu6500.cpp b/src/drivers/mpu6500/mpu6500.cpp index c3457c5b8b..20fcfb6dbe 100644 --- a/src/drivers/mpu6500/mpu6500.cpp +++ b/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); diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 761c8f1008..b2dd1b3549 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/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); diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 095d70e38b..647ae1590c 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/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); diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 26bfc4d9ef..971be328ee 100644 --- a/src/lib/conversion/rotation.cpp +++ b/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) { diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 1e1cdce268..87ac9064d5 100644 --- a/src/lib/conversion/rotation.h +++ b/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 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 48a08c312f..f450b2e9fc 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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); diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index d1027d3b0e..f58e093090 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -46,6 +46,13 @@ // Hack until everything is using this header #include +// 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. */ diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 012db15942..d7084fffcf 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/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);