yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

replay: add backward compatibility for updated sensor_combined topic

Browse Source
This commit is contained in:
Beat Küng
2017-07-14 09:53:00 +02:00
committed by Lorenz Meier
parent cef3a29ac9
commit c59d7baad8
2 changed files with 103 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
src/modules/replay/replay.hpp
View File

@@ -95,6 +95,36 @@ public:
protected:
/**
* @class Compatibility base class to convert topics to an updated format
*/
class CompatBase
{
public:
virtual ~CompatBase() = default;
/**
* apply compatibility to a topic
* @param data input topic (can be modified in place)
* @return new topic data
*/
virtual void *apply(void *data) = 0;
};
class CompatSensorCombinedDtType : public CompatBase
{
public:
CompatSensorCombinedDtType(int gyro_integral_dt_offset_log, int gyro_integral_dt_offset_intern,
int accelerometer_integral_dt_offset_log, int accelerometer_integral_dt_offset_intern);
void *apply(void *data) override;
private:
int _gyro_integral_dt_offset_log;
int _gyro_integral_dt_offset_intern;
int _accelerometer_integral_dt_offset_log;
int _accelerometer_integral_dt_offset_intern;
};
struct Subscription {
const orb_metadata *orb_meta = nullptr; ///< if nullptr, this subscription is invalid
@@ -107,6 +137,8 @@ protected:
std::streampos next_read_pos;
uint64_t next_timestamp; ///< timestamp of the file
CompatBase *compat = nullptr;
// statistics
int error_counter = 0;
int publication_counter = 0;

75
src/modules/replay/replay_main.cpp
View File

@@ -86,6 +86,31 @@ class Replay;
char *Replay::_replay_file = nullptr;
Replay::CompatSensorCombinedDtType::CompatSensorCombinedDtType(int gyro_integral_dt_offset_log,
int gyro_integral_dt_offset_intern,
int accelerometer_integral_dt_offset_log, int accelerometer_integral_dt_offset_intern)
: _gyro_integral_dt_offset_log(gyro_integral_dt_offset_log),
_gyro_integral_dt_offset_intern(gyro_integral_dt_offset_intern),
_accelerometer_integral_dt_offset_log(accelerometer_integral_dt_offset_log),
_accelerometer_integral_dt_offset_intern(accelerometer_integral_dt_offset_intern)
{
}
void *Replay::CompatSensorCombinedDtType::apply(void *data)
{
// the types have the same size so we can do the conversion in-place
uint8_t *ptr = (uint8_t *)data;
float gyro_integral_dt;
float accel_integral_dt;
memcpy(&gyro_integral_dt, ptr + _gyro_integral_dt_offset_log, sizeof(float));
memcpy(&accel_integral_dt, ptr + _accelerometer_integral_dt_offset_log, sizeof(float));
uint32_t igyro_integral_dt = (uint32_t)(gyro_integral_dt * 1e6f);
uint32_t iaccel_integral_dt = (uint32_t)(accel_integral_dt * 1e6f);
memcpy(ptr + _gyro_integral_dt_offset_intern, &igyro_integral_dt, sizeof(float));
memcpy(ptr + _accelerometer_integral_dt_offset_intern, &iaccel_integral_dt, sizeof(float));
return data;
}
void Replay::setupReplayFile(const char *file_name)
{
if (_replay_file) {
@@ -316,20 +341,53 @@ bool Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
return true;
}
CompatBase *compat = nullptr;
//check the format: the field definitions must match
//FIXME: this should check recursively, all used nested types
string file_format = _file_formats[topic_name];
if (file_format != orb_meta->o_fields) {
PX4_WARN("Formats for %s don't match. Will ignore it.", topic_name.c_str());
PX4_WARN(" Internal format: %s", orb_meta->o_fields);
PX4_WARN(" File format : %s", file_format.c_str());
return true; // not a fatal error
// check if we have a compatibility conversion available
if (topic_name == "sensor_combined") {
if (string(orb_meta->o_fields) == "uint64_t timestamp;float[3] gyro_rad;uint32_t gyro_integral_dt;"
"int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;"
"uint32_t accelerometer_integral_dt;int32_t magnetometer_timestamp_relative;"
"float[3] magnetometer_ga;int32_t baro_timestamp_relative;float baro_alt_meter;"
"float baro_temp_celcius;" &&
file_format == "uint64_t timestamp;float[3] gyro_rad;float gyro_integral_dt;"
"int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;"
"float accelerometer_integral_dt;int32_t magnetometer_timestamp_relative;"
"float[3] magnetometer_ga;int32_t baro_timestamp_relative;float baro_alt_meter;"
"float baro_temp_celcius;") {
int gyro_integral_dt_offset_log;
int gyro_integral_dt_offset_intern;
int accelerometer_integral_dt_offset_log;
int accelerometer_integral_dt_offset_intern;
int unused;
if (findFieldOffset(file_format, "gyro_integral_dt", gyro_integral_dt_offset_log, unused) &&
findFieldOffset(orb_meta->o_fields, "gyro_integral_dt", gyro_integral_dt_offset_intern, unused) &&
findFieldOffset(file_format, "accelerometer_integral_dt", accelerometer_integral_dt_offset_log, unused) &&
findFieldOffset(orb_meta->o_fields, "accelerometer_integral_dt", accelerometer_integral_dt_offset_intern, unused)) {
compat = new CompatSensorCombinedDtType(gyro_integral_dt_offset_log, gyro_integral_dt_offset_intern,
accelerometer_integral_dt_offset_log, accelerometer_integral_dt_offset_intern);
}
}
}
if (!compat) {
PX4_WARN("Formats for %s don't match. Will ignore it.", topic_name.c_str());
PX4_WARN(" Internal format: %s", orb_meta->o_fields);
PX4_WARN(" File format : %s", file_format.c_str());
return true; // not a fatal error
}
}
Subscription subscription;
subscription.orb_meta = orb_meta;
subscription.multi_id = multi_id;
subscription.compat = compat;
//find the timestamp offset
@@ -755,6 +813,11 @@ void Replay::run()
}
for (auto &subscription : _subscriptions) {
if (subscription.compat) {
delete subscription.compat;
subscription.compat = nullptr;
}
if (subscription.orb_advert) {
orb_unadvertise(subscription.orb_advert);
subscription.orb_advert = nullptr;
@@ -805,6 +868,10 @@ bool Replay::publishTopic(Subscription &sub, void *data)
{
bool published = false;
if (sub.compat) {
data = sub.compat->apply(data);
}
if (sub.orb_advert) {
orb_publish(sub.orb_meta, sub.orb_advert, data);
published = true;