Browse Source

sensor_baro add separate timestamp_sample field

- the timestamp is uORB message publication metadata
sbg
Daniel Agar 5 years ago
parent
commit
1394b5d7bc
  1. 8
      msg/sensor_baro.msg
  2. 10
      src/lib/drivers/barometer/PX4Barometer.cpp
  3. 2
      src/lib/drivers/barometer/PX4Barometer.hpp
  4. 4
      src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

8
msg/sensor_baro.msg

@ -1,7 +1,9 @@ @@ -1,7 +1,9 @@
uint64 timestamp # time since system start (microseconds)
uint32 device_id # Sensor ID that must be unique for each baro sensor and must not change
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint64 error_count
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint32 error_count
float32 pressure # static pressure measurement in millibar

10
src/lib/drivers/barometer/PX4Barometer.cpp

@ -55,8 +55,7 @@ PX4Barometer::~PX4Barometer() @@ -55,8 +55,7 @@ PX4Barometer::~PX4Barometer()
_sensor_baro_pub.unadvertise();
}
void
PX4Barometer::set_device_type(uint8_t devtype)
void PX4Barometer::set_device_type(uint8_t devtype)
{
// current DeviceStructure
union device::Device::DeviceId device_id;
@ -69,13 +68,12 @@ PX4Barometer::set_device_type(uint8_t devtype) @@ -69,13 +68,12 @@ PX4Barometer::set_device_type(uint8_t devtype)
_sensor_baro_pub.get().device_id = device_id.devid;
}
void
PX4Barometer::update(hrt_abstime timestamp, float pressure)
void PX4Barometer::update(const hrt_abstime &timestamp_sample, float pressure)
{
sensor_baro_s &report = _sensor_baro_pub.get();
report.timestamp = timestamp;
report.timestamp_sample = timestamp_sample;
report.pressure = pressure;
report.timestamp = hrt_absolute_time();
_sensor_baro_pub.update();
}

2
src/lib/drivers/barometer/PX4Barometer.hpp

@ -55,7 +55,7 @@ public: @@ -55,7 +55,7 @@ public:
void set_temperature(float temperature) { _sensor_baro_pub.get().temperature = temperature; }
void update(hrt_abstime timestamp, float pressure);
void update(const hrt_abstime &timestamp_sample, float pressure);
int get_class_instance() { return _class_device_instance; };

4
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@ -200,7 +200,7 @@ void VehicleAirData::Run() @@ -200,7 +200,7 @@ void VehicleAirData::Run()
const sensor_baro_s &baro = _last_data[_selected_sensor_sub_index];
_baro_timestamp_sum += baro.timestamp;
_baro_timestamp_sum += baro.timestamp_sample;
_baro_sum += baro.pressure;
_baro_sum_count++;
@ -217,7 +217,7 @@ void VehicleAirData::Run() @@ -217,7 +217,7 @@ void VehicleAirData::Run()
// populate vehicle_air_data with primary baro and publish
vehicle_air_data_s out{};
out.timestamp_sample = timestamp_sample; // TODO: baro.timestamp_sample;
out.timestamp_sample = timestamp_sample;
out.baro_device_id = baro.device_id;
out.baro_temp_celcius = baro.temperature;

Loading…
Cancel
Save