diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index fab3dcad02..206258e31d 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -23,7 +23,6 @@ uint32 SENSOR_PRIO_MAX = 255 # Actual data, this is specific to the type of data which is stored in this struct # A line containing L0GME will be added by the Python logging code generator to the logged dataset. # -# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only uint64[3] gyro_timestamp # Gyro timestamps int16[9] gyro_raw # Raw sensor values of angular velocity @@ -62,12 +61,3 @@ uint64[3] baro_timestamp # Barometer timestamp uint32[3] baro_priority # Sensor priority uint32[3] baro_errcount # Error count in communication -float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1 -uint16[10] adc_mapping # Channel indices of each of these values -float32 mcu_temp_celcius # Internal temperature measurement of MCU - -float32[3] differential_pressure_pa # Airspeed sensor differential pressure -uint64[3] differential_pressure_timestamp # Last measurement timestamp -float32[3] differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading -uint32[3] differential_pressure_priority # Sensor priority -uint32[3] differential_pressure_errcount # Error count in communication diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b62f150dbc..a20d99294b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -691,6 +692,9 @@ private: MavlinkOrbSubscription *_sensor_sub; uint64_t _sensor_time; + MavlinkOrbSubscription *_differential_pressure_sub; + uint64_t _differential_pressure_time; + uint64_t _accel_timestamp; uint64_t _gyro_timestamp; uint64_t _mag_timestamp; @@ -704,6 +708,8 @@ protected: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), _sensor_time(0), + _differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))), + _differential_pressure_time(0), _accel_timestamp(0), _gyro_timestamp(0), _mag_timestamp(0), @@ -713,6 +719,7 @@ protected: void send(const hrt_abstime t) { struct sensor_combined_s sensor; + struct differential_pressure_s differential_pressure; if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; @@ -740,6 +747,7 @@ protected: fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); _baro_timestamp = sensor.baro_timestamp[0]; } + _differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure); mavlink_highres_imu_t msg; @@ -754,7 +762,7 @@ protected: msg.ymag = sensor.magnetometer_ga[1]; msg.zmag = sensor.magnetometer_ga[2]; msg.abs_pressure = sensor.baro_pres_mbar[0]; - msg.diff_pressure = sensor.differential_pressure_pa[0]; + msg.diff_pressure = differential_pressure.differential_pressure_raw_pa; msg.pressure_alt = sensor.baro_alt_meter[0]; msg.temperature = sensor.baro_temp_celcius[0]; msg.fields_updated = fields_updated; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 52aa816968..c7baa5da05 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1705,10 +1705,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.accelerometer_timestamp[0] = timestamp; hil_sensors.accelerometer_priority[0] = ORB_PRIO_HIGH; - hil_sensors.adc_voltage_v[0] = 0.0f; - hil_sensors.adc_voltage_v[1] = 0.0f; - hil_sensors.adc_voltage_v[2] = 0.0f; - hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f; hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f; hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f; @@ -1726,10 +1722,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.baro_temp_celcius[0] = imu.temperature; hil_sensors.baro_timestamp[0] = timestamp; - hil_sensors.differential_pressure_pa[0] = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_filtered_pa[0] = hil_sensors.differential_pressure_pa[0]; - hil_sensors.differential_pressure_timestamp[0] = timestamp; - /* publish combined sensor topic */ if (_sensors_pub == nullptr) { _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index cd17ae121c..261b319f6e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1374,7 +1374,6 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime accelerometer_timestamp[3] = {0, 0, 0}; hrt_abstime magnetometer_timestamp[3] = {0, 0, 0}; hrt_abstime barometer_timestamp[3] = {0, 0, 0}; - hrt_abstime differential_pressure_timestamp[3] = {0, 0, 0}; /* initialize calculated mean SNR */ float snr_mean = 0.0f; @@ -1659,11 +1658,6 @@ int sdlog2_thread_main(int argc, char *argv[]) write_SENS = true; } - if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) { - differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i]; - write_SENS = true; - } - if (write_IMU) { switch (i) { case 0: @@ -1708,8 +1702,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i]; log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i]; log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i]; - log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i]; - log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i]; + log_msg.body.log_SENS.diff_pres = 0; + log_msg.body.log_SENS.diff_pres_filtered = 0; LOGBUFFER_WRITE_AND_COUNT(SENS); } } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4053fde20e..35fbb2ed7f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1154,10 +1154,6 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) if (updated) { orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); - raw.differential_pressure_pa[0] = _diff_pres.differential_pressure_raw_pa; - raw.differential_pressure_timestamp[0] = _diff_pres.timestamp; - raw.differential_pressure_filtered_pa[0] = _diff_pres.differential_pressure_filtered_pa; - float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG); @@ -1671,11 +1667,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* Read add channels we got */ for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) { - /* Save raw voltage values */ - if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) { - raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); - raw.adc_mapping[i] = buf_adc[i].am_channel; - } /* look for specific channels and process the raw voltage to measurement data */ if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -2145,10 +2136,6 @@ Sensors::task_main() * do advertisements */ raw.timestamp = hrt_absolute_time(); - raw.adc_voltage_v[0] = 0.0f; - raw.adc_voltage_v[1] = 0.0f; - raw.adc_voltage_v[2] = 0.0f; - raw.adc_voltage_v[3] = 0.0f; _battery.reset(&_battery_status);