dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

WIP logger serialization

This commit is contained in:
Daniel Agar 2016-05-03 16:50:12 -04:00 committed by Lorenz Meier
parent 8f5cb4084d
commit 69c1ce1714
23 changed files with 154 additions and 314 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

189
cmake/configs/nuttx_px4fmu-v2_debug.cmake
View File

@ -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")

2
cmake/configs/nuttx_px4fmu-v2_default.cmake
View File

@ -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

70
msg/templates/uorb/msg.cpp.template
View File

@ -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 <drivers/drv_orb_dev.h>
#include <uORB/topics/@(topic_name).h>
void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer)
{
struct @(uorb_struct) *topic = static_cast<struct @(uorb_struct) *>(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]

59
msg/templates/uorb/msg.h.template
View File

@ -68,6 +68,7 @@ topic_name = spec.short_name
#else
#include <string.h>
#endif
#include <uORB/uORB.h>
@##############################
@ -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]@

8
src/drivers/device/integrator.cpp
View File

@ -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;
}

6
src/drivers/device/integrator.h
View File

@ -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);
};

2
src/drivers/l3gd20/l3gd20.cpp
View File

@ -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);

2
src/drivers/lsm303d/lsm303d.cpp
View File

@ -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);

4
src/drivers/mpu6000/mpu6000.cpp
View File

@ -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);

4
src/drivers/mpu6500/mpu6500.cpp
View File

@ -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);

4
src/drivers/mpu9250/mpu9250.cpp
View File

@ -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);

4
src/drivers/px4flow/px4flow.cpp
View File

@ -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);

10
src/lib/conversion/rotation.cpp
View File

@ -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)
{

5
src/lib/conversion/rotation.h
View File

@ -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

3
src/modules/commander/commander.cpp
View File

@ -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 */

43
src/modules/logger/logger.cpp
View File

@ -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;

4
src/modules/logger/logger.h
View File

@ -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;
}
}
};

4
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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);

21
src/modules/uORB/uORB.h
View File

@ -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

10
src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
View File

@ -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

4
src/platforms/posix/drivers/gyrosim/gyrosim.cpp
View File

@ -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);

2
unittests/uorb_unittests/uORBCommunicator_gtests.cpp
View File

@ -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;

8
unittests/uorb_unittests/uORBGtestTopics.hpp
View File

@ -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_