move pressure altitude from baros to sensors module

src/modules/ekf2/ekf2_main.cpp

@@ -57,7 +57,6 @@
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_selection.h>
@@ -521,7 +520,6 @@ void Ekf2::run()
 	int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	int status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection));
-	int sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 	int landing_target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose));
 
 	bool imu_bias_reset_request = false;
@@ -554,8 +552,6 @@ void Ekf2::run()
 	vehicle_attitude_s ev_att = {};
 	vehicle_status_s vehicle_status = {};
 	sensor_selection_s sensor_selection = {};
-	sensor_baro_s sensor_baro = {};
-	sensor_baro.pressure = 1013.5f; // initialise pressure to sea level
 	landing_target_pose_s landing_target_pose = {};
 
 	while (!should_exit()) {
@@ -588,7 +584,6 @@ void Ekf2::run()
 
 		bool gps_updated = false;
 		bool airspeed_updated = false;
-		bool baro_updated = false;
 		bool sensor_selection_updated = false;
 		bool optical_flow_updated = false;
 		bool range_finder_updated = false;
@@ -620,12 +615,6 @@ void Ekf2::run()
 			orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
 		}
 
-		orb_check(sensor_baro_sub, &baro_updated);
-
-		if (baro_updated) {
-			orb_copy(ORB_ID(sensor_baro), sensor_baro_sub, &sensor_baro);
-		}
-
 		orb_check(sensor_selection_sub, &sensor_selection_updated);
 
 		// Always update sensor selction first time through if time stamp is non zero
@@ -808,8 +797,8 @@ void Ekf2::run()
 					float balt_data_avg = _balt_data_sum / (float)_balt_sample_count;
 
 					// estimate air density assuming typical 20degC ambient temperature
-					const float pressure_to_density = 100.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
-					const float rho = pressure_to_density * sensor_baro.pressure;
+					const float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
+					const float rho = pressure_to_density * sensors.baro_pressure_pa;
 					_ekf.set_air_density(rho);
 
 					// calculate static pressure error = Pmeas - Ptruth
@@ -1454,7 +1443,6 @@ void Ekf2::run()
 	orb_unsubscribe(vehicle_land_detected_sub);
 	orb_unsubscribe(status_sub);
 	orb_unsubscribe(sensor_selection_sub);
-	orb_unsubscribe(sensor_baro_sub);
 	orb_unsubscribe(landing_target_pose_sub);
 
 	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {

src/modules/logger/logger.cpp

@@ -680,7 +680,6 @@ void Logger::add_estimator_replay_topics()
 	add_topic("airspeed");
 	add_topic("distance_sensor");
 	add_topic("optical_flow");
-	add_topic("sensor_baro");
 	add_topic("sensor_combined");
 	add_topic("sensor_selection");
 	add_topic("vehicle_gps_position");

src/modules/mavlink/mavlink_receiver.cpp

@@ -1965,7 +1965,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 		baro.timestamp = timestamp;
 		baro.pressure = imu.abs_pressure;
-		baro.altitude = imu.pressure_alt;
 		baro.temperature = imu.temperature;
 
 		/* fake device ID */

src/modules/sensors/sensors.cpp

@@ -267,30 +267,6 @@ Sensors::parameters_update()
 	_rc_update.update_rc_functions();
 	_voted_sensors_update.parameters_update();
 
-	/* update barometer qnh setting */
-	DevHandle h_baro;
-	DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro);
-
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) && !defined(__PX4_POSIX_OCPOC)
-
-	// TODO: this needs fixing for QURT and Raspberry Pi
-	if (!h_baro.isValid()) {
-		if (!_hil_enabled) { // in HIL we don't have a baro
-			PX4_ERR("no barometer found on %s (%d)", BARO0_DEVICE_PATH, h_baro.getError());
-			ret = PX4_ERROR;
-		}
-
-	} else {
-		int baroret = h_baro.ioctl(BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
-
-		if (baroret) {
-			PX4_ERR("qnh for baro could not be set");
-			ret = PX4_ERROR;
-		}
-	}
-
-#endif
-
 	return ret;
 }
 
@@ -359,16 +335,15 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		airspeed.indicated_airspeed_m_s = math::max(0.0f,
 						  calc_indicated_airspeed_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel,
 								  smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
-								  diff_pres.differential_pressure_filtered_pa, _voted_sensors_update.baro_pressure(),
+								  diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa,
 								  air_temperature_celsius));
 
 		airspeed.true_airspeed_m_s = math::max(0.0f,
-						       calc_true_airspeed_from_indicated(airspeed.indicated_airspeed_m_s,
-								       _voted_sensors_update.baro_pressure(), air_temperature_celsius));
+						       calc_true_airspeed_from_indicated(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa, air_temperature_celsius));
 
 		airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
-							calc_true_airspeed(diff_pres.differential_pressure_raw_pa + _voted_sensors_update.baro_pressure(),
-									_voted_sensors_update.baro_pressure(), air_temperature_celsius));
+							calc_true_airspeed(diff_pres.differential_pressure_raw_pa + raw.baro_pressure_pa, raw.baro_pressure_pa,
+									air_temperature_celsius));
 
 		airspeed.air_temperature_celsius = air_temperature_celsius;
 

src/modules/sensors/voted_sensors_update.cpp

@@ -42,17 +42,14 @@
 #include <systemlib/mavlink_log.h>
 
 #include <conversion/rotation.h>
+#include <geo/geo.h>
 
 #define MAG_ROT_VAL_INTERNAL		-1
 #define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
 
-
 using namespace sensors;
 using namespace DriverFramework;
 
-
-const double VotedSensorsUpdate::_msl_pressure = 101.325;
-
 VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled)
 	: _parameters(parameters), _hil_enabled(hil_enabled)
 {
@@ -551,9 +548,9 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
 		if (accel_updated && _accel.enabled[uorb_index]) {
 			struct accel_report accel_report;
 
-			orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report);
+			int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report);
 
-			if (accel_report.timestamp == 0) {
+			if (ret != PX4_OK || accel_report.timestamp == 0) {
 				continue; //ignore invalid data
 			}
 
@@ -629,6 +626,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
 	// write the best sensor data to the output variables
 	if (best_index >= 0) {
 		raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
+		memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[best_index].accelerometer_m_s2, sizeof(raw.accelerometer_m_s2));
 
 		if (best_index != _accel.last_best_vote) {
 			_accel.last_best_vote = (uint8_t)best_index;
@@ -640,10 +638,6 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
 			_selection_changed = true;
 			_selection.accel_device_id = _accel_device_id[best_index];
 		}
-
-		for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
-			raw.accelerometer_m_s2[axis_index] = _last_sensor_data[best_index].accelerometer_m_s2[axis_index];
-		}
 	}
 }
 
@@ -659,9 +653,9 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
 		if (gyro_updated && _gyro.enabled[uorb_index]) {
 			struct gyro_report gyro_report;
 
-			orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report);
+			int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report);
 
-			if (gyro_report.timestamp == 0) {
+			if (ret != PX4_OK || gyro_report.timestamp == 0) {
 				continue; //ignore invalid data
 			}
 
@@ -736,8 +730,9 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
 
 	// write data for the best sensor to output variables
 	if (best_index >= 0) {
-		raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
 		raw.timestamp = _last_sensor_data[best_index].timestamp;
+		raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
+		memcpy(&raw.gyro_rad, &_last_sensor_data[best_index].gyro_rad, sizeof(raw.gyro_rad));
 
 		if (_gyro.last_best_vote != best_index) {
 			_gyro.last_best_vote = (uint8_t)best_index;
@@ -749,10 +744,6 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
 			_selection_changed = true;
 			_selection.gyro_device_id = _gyro_device_id[best_index];
 		}
-
-		for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
-			raw.gyro_rad[axis_index] = _last_sensor_data[best_index].gyro_rad[axis_index];
-		}
 	}
 }
 
@@ -765,19 +756,14 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
 		if (mag_updated && _mag.enabled[uorb_index]) {
 			struct mag_report mag_report;
 
-			orb_copy(ORB_ID(sensor_mag), _mag.subscription[uorb_index], &mag_report);
+			int ret = orb_copy(ORB_ID(sensor_mag), _mag.subscription[uorb_index], &mag_report);
 
-			if (mag_report.timestamp == 0) {
+			if (ret != PX4_OK || mag_report.timestamp == 0) {
 				continue; //ignore invalid data
 			}
 
 			// First publication with data
 			if (_mag.priority[uorb_index] == 0) {
-
-				// Parameters update to get offsets, scaling & mag rotation loaded (if not already loaded)
-				parameters_update();
-
-				// Set device priority for the voter
 				int32_t priority = 0;
 				orb_priority(_mag.subscription[uorb_index], &priority);
 				_mag.priority[uorb_index] = (uint8_t)priority;
@@ -799,15 +785,13 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
 	_mag.voter.get_best(hrt_absolute_time(), &best_index);
 
 	if (best_index >= 0) {
-		raw.magnetometer_ga[0] = _last_sensor_data[best_index].magnetometer_ga[0];
-		raw.magnetometer_ga[1] = _last_sensor_data[best_index].magnetometer_ga[1];
-		raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2];
+		memcpy(&raw.magnetometer_ga, &_last_sensor_data[best_index].magnetometer_ga, sizeof(raw.magnetometer_ga));
 		_mag.last_best_vote = (uint8_t)best_index;
-	}
 
-	if (_selection.mag_device_id != _mag_device_id[best_index]) {
-		_selection_changed = true;
-		_selection.mag_device_id = _mag_device_id[best_index];
+		if (_selection.mag_device_id != _mag_device_id[best_index]) {
+			_selection_changed = true;
+			_selection.mag_device_id = _mag_device_id[best_index];
+		}
 	}
 }
 
@@ -824,9 +808,9 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
 		if (baro_updated) {
 			struct baro_report baro_report;
 
-			orb_copy(ORB_ID(sensor_baro), _baro.subscription[uorb_index], &baro_report);
+			int ret = orb_copy(ORB_ID(sensor_baro), _baro.subscription[uorb_index], &baro_report);
 
-			if (baro_report.timestamp == 0) {
+			if (ret != PX4_OK || baro_report.timestamp == 0) {
 				continue; //ignore invalid data
 			}
 
@@ -851,14 +835,13 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
 			_baro_device_id[uorb_index] = baro_report.device_id;
 
 			got_update = true;
-			matrix::Vector3f vect(baro_report.altitude, 0.f, 0.f);
+			matrix::Vector3f vect(baro_report.pressure, 0.f, 0.f);
 
-			_last_sensor_data[uorb_index].baro_alt_meter = baro_report.altitude;
 			_last_sensor_data[uorb_index].baro_temp_celcius = baro_report.temperature;
-			_last_baro_pressure[uorb_index] = corrected_pressure;
+			_last_sensor_data[uorb_index].baro_pressure_pa = corrected_pressure;
 
 			_last_baro_timestamp[uorb_index] = baro_report.timestamp;
-			_baro.voter.put(uorb_index, baro_report.timestamp, vect.data(), baro_report.error_count, _baro.priority[uorb_index]);
+			_baro.voter.put(uorb_index, baro_report.timestamp, vect.data, baro_report.error_count, _baro.priority[uorb_index]);
 		}
 	}
 
@@ -868,7 +851,7 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
 
 		if (best_index >= 0) {
 			raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius;
-			_last_best_baro_pressure = _last_baro_pressure[best_index];
+			raw.baro_pressure_pa = _last_sensor_data[best_index].baro_pressure_pa;
 
 			if (_baro.last_best_vote != best_index) {
 				_baro.last_best_vote = (uint8_t)best_index;
@@ -881,31 +864,16 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
 				_selection.baro_device_id = _baro_device_id[best_index];
 			}
 
-			/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
+			// calculate altitude using the hypsometric equation
 
-			/*
-			 * PERFORMANCE HINT:
-			 *
-			 * The single precision calculation is 50 microseconds faster than the double
-			 * precision variant. It is however not obvious if double precision is required.
-			 * Pending more inspection and tests, we'll leave the double precision variant active.
-			 *
-			 * Measurements:
-			 * 	double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
-			 *	single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
-			 */
+			static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS;	/* temperature at base height in Kelvin */
+			static constexpr float a  = -6.5f / 1000.0f;	/* temperature gradient in degrees per metre */
 
-			/* tropospheric properties (0-11km) for standard atmosphere */
-			const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
-			const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
-			const double g  = 9.80665;	/* gravity constant in m/s/s */
-			const double R  = 287.05;	/* ideal gas constant in J/kg/K */
-
-			/* current pressure at MSL in kPa */
-			const double p1 = _msl_pressure;
+			/* current pressure at MSL in kPa (QNH in hPa)*/
+			const float p1 = _parameters.baro_qnh * 0.1f;
 
 			/* measured pressure in kPa */
-			const double p = 0.001f * _last_best_baro_pressure;
+			const float p = raw.baro_pressure_pa * 0.001f;
 
 			/*
 			 * Solve:
@@ -916,7 +884,7 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
 			 * h = -------------------------------  + h1
 			 *                   a
 			 */
-			raw.baro_alt_meter = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+			raw.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
 
 		}
 	}

src/modules/sensors/voted_sensors_update.h

@@ -97,9 +97,6 @@ public:
 
 	void print_status();
 
-	/** get the latest baro pressure */
-	float baro_pressure() const { return _last_best_baro_pressure; }
-
 	/**
 	 * call this whenever parameters got updated. Make sure to have initialize_sensors() called at least
 	 * once before calling this.
@@ -246,9 +243,8 @@ private:
 
 	orb_advert_t	_mavlink_log_pub = nullptr;
 
-	float _last_baro_pressure[BARO_COUNT_MAX]; /**< pressure from last baro sensors */
-	float _last_best_baro_pressure = 0.0f; /**< pressure from last best baro */
 	sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */
+
 	uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX]; /**< latest full timestamp */
 	uint64_t _last_mag_timestamp[MAG_COUNT_MAX]; /**< latest full timestamp */
 	uint64_t _last_baro_timestamp[BARO_COUNT_MAX]; /**< latest full timestamp */
@@ -273,12 +269,12 @@ private:
 	struct sensor_selection_s _selection = {}; /**< struct containing the sensor selection to be published to the uORB*/
 	orb_advert_t _sensor_selection_pub = nullptr; /**< handle to the sensor selection uORB topic */
 	bool _selection_changed = false; /**< true when a sensor selection has changed and not been published */
+
 	uint32_t _accel_device_id[SENSOR_COUNT_MAX] = {}; /**< accel driver device id for each uorb instance */
 	uint32_t _baro_device_id[SENSOR_COUNT_MAX] = {};
 	uint32_t _gyro_device_id[SENSOR_COUNT_MAX] = {};
 	uint32_t _mag_device_id[SENSOR_COUNT_MAX] = {};
 
-	static const double	_msl_pressure;	/** average sea-level pressure in kPa */
 };
 
 

src/modules/simulator/simulator_mavlink.cpp

@@ -1073,7 +1073,6 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
 
 		baro.timestamp = timestamp;
 		baro.pressure = imu->abs_pressure;
-		baro.altitude = imu->pressure_alt;
 		baro.temperature = imu->temperature;
 
 		int baro_multi;

src/modules/uavcan/sensors/baro.cpp

@@ -98,21 +98,6 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl
 int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
-	case BAROIOCSMSLPRESSURE: {
-			if ((arg < 80000) || (arg > 120000)) {
-				return -EINVAL;
-
-			} else {
-				DEVICE_LOG("new msl pressure %u", _msl_pressure);
-				_msl_pressure = arg;
-				return OK;
-			}
-		}
-
-	case BAROIOCGMSLPRESSURE: {
-			return _msl_pressure;
-		}
-
 	case SENSORIOCSPOLLRATE: {
 			// not supported yet, pretend that everything is ok
 			return OK;
@@ -168,20 +153,6 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 	/* TODO get device ID for sensor */
 	report.device_id = 0;
 
-	/*
-	 * Altitude computation
-	 * Refer to the MS5611 driver for details
-	 */
-	const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin
-	const double a  = -6.5 / 1000;   // temperature gradient in degrees per metre
-	const double g  = 9.80665;       // gravity constant in m/s/s
-	const double R  = 287.05;        // ideal gas constant in J/kg/K
-
-	const double p1 = _msl_pressure / 1000.0;      // current pressure at MSL in kPa
-	const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
-
-	report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-
 	// add to the ring buffer
 	_reports.force(&report);
 

src/modules/uavcan/sensors/baro.hpp

@@ -79,7 +79,6 @@ private:
 
 	ringbuffer::RingBuffer _reports;
 
-	unsigned _msl_pressure = 101325;
 	float last_temperature_kelvin = 0.0f;
 
 };