diff --git a/cmake/configs/nuttx_px4fmu-v2_debug.cmake b/cmake/configs/nuttx_px4fmu-v2_debug.cmake deleted file mode 100644 index e0fedbb7e6..0000000000 --- a/cmake/configs/nuttx_px4fmu-v2_debug.cmake +++ /dev/null @@ -1,189 +0,0 @@ -set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "build type") - - -include(nuttx/px4_impl_nuttx) - -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) - -set(config_module_list - # - # Board support modules - # - drivers/device - drivers/stm32 - drivers/stm32/adc - drivers/stm32/tone_alarm - drivers/led - drivers/px4fmu - drivers/px4io - drivers/boards/px4fmu-v2 - drivers/rgbled - drivers/mpu6000 - #drivers/mpu9250 - drivers/lsm303d - drivers/l3gd20 - #drivers/hmc5883 - drivers/ms5611 - #drivers/mb12xx - #drivers/srf02 - #drivers/sf0x - #drivers/ll40ls - drivers/trone - drivers/gps - drivers/pwm_out_sim - #drivers/hott - #drivers/hott/hott_telemetry - #drivers/hott/hott_sensors - #drivers/blinkm - drivers/airspeed - drivers/ets_airspeed - drivers/meas_airspeed - #drivers/frsky_telemetry - modules/sensors - #drivers/mkblctrl - #drivers/px4flow - #drivers/oreoled - drivers/gimbal - drivers/pwm_input - drivers/camera_trigger - drivers/bst - #drivers/snapdragon_rc_pwm - #drivers/lis3mdl - - # - # System commands - # - systemcmds/bl_update - systemcmds/mixer - systemcmds/param - systemcmds/perf - systemcmds/pwm - systemcmds/esc_calib - systemcmds/reboot - #systemcmds/topic_listener - systemcmds/top - systemcmds/config - systemcmds/nshterm - systemcmds/mtd - systemcmds/dumpfile - systemcmds/ver - - # - # General system control - # - modules/commander - modules/navigator - modules/mavlink - modules/gpio_led - #modules/uavcan - modules/land_detector - - # - # Estimation modules (EKF/ SO3 / other filters) - # - # Too high RAM usage due to static allocations - # modules/attitude_estimator_ekf - modules/attitude_estimator_q - modules/ekf_att_pos_estimator - modules/position_estimator_inav - - # - # Vehicle Control - # - # modules/segway # XXX Needs GCC 4.7 fix - modules/fw_pos_control_l1 - modules/fw_att_control - modules/mc_att_control - modules/mc_pos_control - modules/vtol_att_control - - # - # Logging - # - modules/sdlog2 - - # - # Library modules - # - modules/param - modules/systemlib - modules/systemlib/mixer - modules/uORB - modules/dataman - - # - # Libraries - # - lib/controllib - lib/mathlib - lib/mathlib/math/filter - lib/ecl - lib/external_lgpl - lib/geo - lib/geo_lookup - lib/conversion - lib/launchdetection - lib/terrain_estimation - lib/runway_takeoff - lib/tailsitter_recovery - platforms/nuttx - - # had to add for cmake, not sure why wasn't in original config - platforms/common - platforms/nuttx/px4_layer - - # - # OBC challenge - # - #modules/bottle_drop - - # - # Rover apps - # - #examples/rover_steering_control - - # - # Demo apps - # - #examples/math_demo - # Tutorial code from - # https://px4.io/dev/px4_simple_app - #examples/px4_simple_app - - # Tutorial code from - # https://px4.io/dev/daemon - #examples/px4_daemon_app - - # Tutorial code from - # https://px4.io/dev/debug_values - #examples/px4_mavlink_debug - - # Tutorial code from - # https://px4.io/dev/example_fixedwing_control - #examples/fixedwing_control - - # Hardware test - #examples/hwtest -) - -set(config_extra_builtin_cmds - serdis - sercon - ) - -set(config_io_board - px4io-v2 - ) - -set(config_io_extra_libs - ) - -add_custom_target(sercon) -set_target_properties(sercon PROPERTIES - PRIORITY "SCHED_PRIORITY_DEFAULT" - MAIN "sercon" STACK_MAIN "2048") - -add_custom_target(serdis) -set_target_properties(serdis PROPERTIES - PRIORITY "SCHED_PRIORITY_DEFAULT" - MAIN "serdis" STACK_MAIN "2048") diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index ee1263bad6..71cdc9463a 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -85,7 +85,7 @@ set(config_module_list modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav - modules/local_position_estimator + #modules/local_position_estimator # # Vehicle Control diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template index 54313dc5ec..589d05dca4 100644 --- a/msg/templates/uorb/msg.cpp.template +++ b/msg/templates/uorb/msg.cpp.template @@ -57,7 +57,8 @@ uorb_struct = '%s_s'%spec.short_name uorb_pack_func = 'pack_%s'%spec.short_name topic_name = spec.short_name -type_map = {'int8': 'int8_t', +type_map = { + 'int8': 'int8_t', 'int16': 'int16_t', 'int32': 'int32_t', 'int64': 'int64_t', @@ -71,23 +72,28 @@ type_map = {'int8': 'int8_t', 'char': 'char', 'fence_vertex': 'fence_vertex', 'position_setpoint': 'position_setpoint', - 'esc_report': 'esc_report'} + 'esc_report': 'esc_report' +} -msgtype_size_map = {'int8': 8, - 'int16': 16, - 'int32': 32, - 'int64': 64, - 'uint8': 8, - 'uint16': 16, - 'uint32': 32, - 'uint64': 64, - 'float32': 32, - 'float64': 64, +msgtype_size_map = { + 'int8': 1, + 'int16': 2, + 'int32': 4, + 'int64': 8, + 'uint8': 1, + 'uint16': 2, + 'uint32': 4, + 'uint64': 8, + 'float32': 4, + 'float64': 8, 'bool': 1, 'char': 1, 'fence_vertex': 8, 'position_setpoint': 104, - 'esc_report': 36} + 'esc_report': 36 +} + +msgtypes_composite = ['position_setpoint', 'esc_report', 'fence_vertex'] def convert_type(spec_type): bare_type = spec_type @@ -112,7 +118,7 @@ def bare_name(msg_type): def sizeof_field_type(field): return msgtype_size_map[bare_name(field.type)] -sorted_fields = sorted(spec.parsed_fields(), key = sizeof_field_type, reverse=True) +sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields] }@ @@ -120,9 +126,43 @@ topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in so #include #include + +void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer) +{ + struct @(uorb_struct) *topic = static_cast(in); + +@{ + +serialize_expr = " serialize_{type}(&topic->{field}, buffer);" +serialize_array_expr = " serialize_{type}(&topic->{field}[{index}], buffer);" + +buffer_memcpy_expr = " memcpy(((char *)buffer->data) + buffer->next, &topic->{field}, sizeof(topic->{field}));" +buffer_next_expr = " buffer->next += sizeof(topic->{field});" + +for each_field in sorted_fields: + if not each_field.is_header: + + # call appropriate serialize functions for fields that are orb messages + if bare_name(each_field.type) in msgtypes_composite: + + if each_field.is_array: + for i in range(each_field.array_len): + print(serialize_array_expr.format(type=type_map[bare_name(each_field.type)], field=each_field.name, index=i)) + + else: + print(serialize_expr.format(type=type_map[bare_name(each_field.type)], field=each_field.name)) + + # copy primitive fields + else: + print(buffer_memcpy_expr.format(field=each_field.name)) + print(buffer_next_expr.format(field=each_field.name)) + +}@ +} + @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed" const char *__orb_@(topic_name)_fields = "@( topic_name.upper() ):@( ";".join(topic_fields) );"; @[for multi_topic in topics]@ -ORB_DEFINE(@multi_topic, struct @uorb_struct, __orb_@(topic_name)_fields); +ORB_DEFINE(@multi_topic, struct @uorb_struct, &serialize_@(topic_name), __orb_@(topic_name)_fields); @[end for] diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template index 8ad98c61d5..9196edbc4d 100644 --- a/msg/templates/uorb/msg.h.template +++ b/msg/templates/uorb/msg.h.template @@ -68,6 +68,7 @@ topic_name = spec.short_name #else #include #endif + #include @############################## @@ -93,7 +94,8 @@ for field in spec.parsed_fields(): @# Main struct of message @############################## @{ -type_map = {'int8': 'int8_t', +type_map = { + 'int8': 'int8_t', 'int16': 'int16_t', 'int32': 'int32_t', 'int64': 'int64_t', @@ -111,16 +113,16 @@ type_map = {'int8': 'int8_t', } msgtype_size_map = { - 'int8': 8, - 'int16': 16, - 'int32': 32, - 'int64': 64, - 'uint8': 8, - 'uint16': 16, - 'uint32': 32, - 'uint64': 64, - 'float32': 32, - 'float64': 64, + 'int8': 1, + 'int16': 2, + 'int32': 4, + 'int64': 8, + 'uint8': 1, + 'uint16': 2, + 'uint32': 4, + 'uint64': 8, + 'float32': 4, + 'float64': 8, 'bool': 1, 'char': 1, 'fence_vertex': 8, @@ -128,33 +130,6 @@ msgtype_size_map = { 'esc_report': 36 } -def bare_name(msg_type): - bare = msg_type - if '/' in msg_type: - # removing prefix - bare = (msg_type.split('/'))[1] - # removing suffix - return bare.split('[')[0] - -def sizeof_field_type(field): - return msgtype_size_map[bare_name(field.type)] - -msgtype_size_map = {'int8': 8, - 'int16': 16, - 'int32': 32, - 'int64': 64, - 'uint8': 8, - 'uint16': 16, - 'uint32': 32, - 'uint64': 64, - 'float32': 32, - 'float64': 64, - 'bool': 1, - 'char': 1, - 'fence_vertex': 8, - 'position_setpoint': 104, - 'esc_report': 36} - def bare_name(msg_type): bare = msg_type if '/' in msg_type: @@ -196,7 +171,7 @@ def print_field_def(field): def print_parsed_fields(): # sort fields - sorted_fields = sorted(spec.parsed_fields(), key = sizeof_field_type, reverse=True) + sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) # loop over all fields and print the type and name for field in sorted_fields: if (not field.is_header): @@ -204,7 +179,6 @@ def print_parsed_fields(): }@ -#pragma pack(push, 1) #ifdef __cplusplus @#class @(uorb_struct) { struct __EXPORT @(uorb_struct) { @@ -230,11 +204,8 @@ for constant in spec.constants: } #endif }; -#pragma pack(pop) -/** - * @@} - */ +void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer); /* register this as object request broker structure */ @[for multi_topic in topics]@ diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp index 86a9b53505..072e3096a2 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 86817323e2..48a232066f 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 e0b2c39c80..61345628b1 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 6563282f4e..71796cfe8c 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 a6b2b2e6f1..4bf6384b3f 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 20fcfb6dbe..c3457c5b8b 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 b2dd1b3549..761c8f1008 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 647ae1590c..095d70e38b 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 971be328ee..26bfc4d9ef 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -52,16 +52,6 @@ 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 87ac9064d5..1e1cdce268 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -130,11 +130,6 @@ 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/commander/commander.cpp b/src/modules/commander/commander.cpp index bb743239b9..ba304dd229 100644 --- a/src/modules/commander/commander.cpp +++ b/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 */ diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index a2345a86f6..a323b8407b 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -4,8 +4,11 @@ #include #include +#include #include +#include #include +#include #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(×tamp, buffer + sizeof(message_data_header_s), sizeof(timestamp)); @@ -505,10 +528,14 @@ void Logger::run() */ message_data_header_s *header = reinterpret_cast(buffer); header->msg_type = static_cast(MessageType::DATA); - header->msg_size = static_cast(msg_size - 3); + /* the ORB topic data object has 2 unused trailing bytes? */ + //header->msg_size = static_cast(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; diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 4a0dc59d04..3309ebcb1d 100644 --- a/src/modules/logger/logger.h +++ b/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; + } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f450b2e9fc..7a72c24ded 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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); diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index d9e57c9436..1b06a2ad89 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -46,12 +46,12 @@ // 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 +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 diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp index 032b25a544..6f5012cec3 100644 --- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp +++ b/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 diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index d7084fffcf..012db15942 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); diff --git a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp index 35822f016f..a0b61a250f 100644 --- a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp +++ b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp @@ -206,7 +206,7 @@ namespace uORB_test ASSERT_EQ( c._send_messageCount, 0 ); //step 1. - ORB_DEFINE( topicA_sndmsg, struct orb_topic_A, "TOPICA_SNDMSG:int16_t val;" ); + ORB_DEFINE( topicA_sndmsg, struct orb_topic_A, nullptr, "TOPICA_SNDMSG:int16_t val;" ); _topicA.val = 1; _pub_ptr = orb_advertise(ORB_ID(topicA_sndmsg ), &_topicA ); ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic topicA_sndmsg: errno: " << errno; diff --git a/unittests/uorb_unittests/uORBGtestTopics.hpp b/unittests/uorb_unittests/uORBGtestTopics.hpp index 0cea8d55fc..ca8572efac 100644 --- a/unittests/uorb_unittests/uORBGtestTopics.hpp +++ b/unittests/uorb_unittests/uORBGtestTopics.hpp @@ -48,11 +48,11 @@ namespace uORB_test int16_t val; }; - ORB_DEFINE( topicA, struct orb_topic_A, "TOPICA:int16 val;" ); - ORB_DEFINE( topicB, struct orb_topic_B, "TOPICB:int16 val;" ); + ORB_DEFINE(topicA, struct orb_topic_A, nullptr, "TOPICA:int16 val;"); + ORB_DEFINE(topicB, struct orb_topic_B, nullptr, "TOPICB:int16 val;"); - ORB_DEFINE( topicA_clone, struct orb_topic_A, "TOPICA_CLONE:int16 val;" ); - ORB_DEFINE( topicB_clone, struct orb_topic_B, "TOPICB_CLONE:int16 val;" ); + ORB_DEFINE(topicA_clone, struct orb_topic_A, nullptr, "TOPICA_CLONE:int16 val;"; + ORB_DEFINE(topicB_clone, struct orb_topic_B, nullptr, "TOPICB_CLONE:int16 val;"); } #endif // _UnitTest_uORBTopics_hpp_