sensors.cpp 23.7 KB
Newer Older
1
2
/****************************************************************************
 *
3
 *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file sensors.cpp
36
 *
37
 * @author Lorenz Meier <lorenz@px4.io>
38
 * @author Julian Oes <julian@oes.ch>
39
 * @author Thomas Gubler <thomas@px4.io>
40
 * @author Anton Babushkin <anton@px4.io>
41
 * @author Beat Küng <beat-kueng@gmx.net>
42
43
 */

44
45
46
47
48
49
50
#include <drivers/drv_adc.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <lib/airspeed/airspeed.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
51
#include <lib/sensor_calibration/Utilities.hpp>
52
#include <px4_platform_common/getopt.h>
53
54
55
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
56
#include <px4_platform_common/px4_config.h>
Daniel Agar's avatar
Daniel Agar committed
57
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
58
59
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/time.h>
60
61
62
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
Daniel Agar's avatar
Daniel Agar committed
63
#include <uORB/SubscriptionCallback.hpp>
64
#include <uORB/topics/actuator_controls.h>
65
#include <uORB/topics/airspeed.h>
66
67
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/parameter_update.h>
68
#include <uORB/topics/sensors_status_imu.h>
69
#include <uORB/topics/vehicle_air_data.h>
70
#include <uORB/topics/vehicle_control_mode.h>
71
#include <uORB/topics/vehicle_imu.h>
72

73
#include "voted_sensors_update.h"
74
#include "vehicle_acceleration/VehicleAcceleration.hpp"
75
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
76
#include "vehicle_air_data/VehicleAirData.hpp"
77
#include "vehicle_gps_position/VehicleGPSPosition.hpp"
78
#include "vehicle_imu/VehicleIMU.hpp"
79
#include "vehicle_magnetometer/VehicleMagnetometer.hpp"
80

81
using namespace sensors;
82
using namespace time_literals;
83

Simon Wilks's avatar
Cleanup    
Simon Wilks committed
84
85
86
87
/**
 * HACK - true temperature is much less than indicated temperature in baro,
 * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
 */
88
#define PCB_TEMP_ESTIMATE_DEG		5.0f
Daniel Agar's avatar
Daniel Agar committed
89
class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::ScheduledWorkItem
90
91
{
public:
92
	explicit Sensors(bool hil_enabled);
93
	~Sensors() override;
94

95
96
	/** @see ModuleBase */
	static int task_spawn(int argc, char *argv[]);
97

98
99
100
101
102
103
104
	/** @see ModuleBase */
	static int custom_command(int argc, char *argv[]);

	/** @see ModuleBase */
	static int print_usage(const char *reason = nullptr);

	/** @see ModuleBase::run() */
Daniel Agar's avatar
Daniel Agar committed
105
	void Run() override;
106
107
108

	/** @see ModuleBase::print_status() */
	int print_status() override;
109

Daniel Agar's avatar
Daniel Agar committed
110
111
	bool init();

112
private:
113
	const bool	_hil_enabled;			/**< if true, HIL is active */
Daniel Agar's avatar
Daniel Agar committed
114
	bool		_armed{false};				/**< arming status of the vehicle */
115

Daniel Agar's avatar
Daniel Agar committed
116
117
118
119
120
	hrt_abstime     _last_config_update{0};
	hrt_abstime     _sensor_combined_prev_timestamp{0};

	sensor_combined_s _sensor_combined{};

121
	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[MAX_SENSOR_COUNT] {
122
123
		{this, ORB_ID(vehicle_imu), 0},
		{this, ORB_ID(vehicle_imu), 1},
124
125
		{this, ORB_ID(vehicle_imu), 2},
		{this, ORB_ID(vehicle_imu), 3}
126
127
	};

128
129
	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

130
131
132
	uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
133

134
135
	uORB::Publication<airspeed_s>             _airspeed_pub{ORB_ID(airspeed)};
	uORB::Publication<sensor_combined_s>      _sensor_pub{ORB_ID(sensor_combined)};
136

px4dev's avatar
px4dev committed
137
	perf_counter_t	_loop_perf;			/**< loop performance counter */
138

139
140
	DataValidator	_airspeed_validator;		/**< data validator to monitor airspeed */

141
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
142
143
144

	hrt_abstime	_last_adc{0};			/**< last time we took input from the ADC */

145
	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};		/**< adc_report sub */
146
	differential_pressure_s	_diff_pres {};
147
	uORB::PublicationMulti<differential_pressure_s>	_diff_pres_pub{ORB_ID(differential_pressure)};		/**< differential_pressure */
Daniel Agar's avatar
Daniel Agar committed
148
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
149

150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171

	struct Parameters {
		float diff_pres_offset_pa;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
		float diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */

		int32_t air_cmodel;
		float air_tube_length;
		float air_tube_diameter_mm;
	} _parameters{}; /**< local copies of interesting parameters */

	struct ParameterHandles {
		param_t diff_pres_offset_pa;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
		param_t diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */

		param_t air_cmodel;
		param_t air_tube_length;
		param_t air_tube_diameter_mm;
	} _parameter_handles{};		/**< handles for interesting parameters */
172

173
	VotedSensorsUpdate _voted_sensors_update;
174

175
176
	VehicleAcceleration	_vehicle_acceleration;
	VehicleAngularVelocity	_vehicle_angular_velocity;
177
	VehicleAirData          *_vehicle_air_data{nullptr};
178
	VehicleMagnetometer     *_vehicle_magnetometer{nullptr};
179
	VehicleGPSPosition	*_vehicle_gps_position{nullptr};
180

181
182
	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};

183
184
185
186
187
188
	uint8_t _n_accel{0};
	uint8_t _n_baro{0};
	uint8_t _n_gps{0};
	uint8_t _n_gyro{0};
	uint8_t _n_mag{0};

px4dev's avatar
px4dev committed
189
190
191
	/**
	 * Update our local parameter cache.
	 */
192
193
	int		parameters_update();

194
195
196
197
198
199
	/**
	 * Poll the differential pressure sensor for updated data.
	 *
	 * @param raw			Combined sensor data structure into which
	 *				data should be returned.
	 */
200
	void		diff_pres_poll();
201

202
203
204
	/**
	 * Check for changes in parameters.
	 */
205
	void 		parameter_update_poll(bool forced = false);
206

px4dev's avatar
px4dev committed
207
208
209
210
211
212
	/**
	 * Poll the ADC and update readings to suit.
	 *
	 * @param raw			Combined sensor data structure into which
	 *				data should be returned.
	 */
213
	void		adc_poll();
214

215
	void		InitializeVehicleAirData();
216
	void		InitializeVehicleGPSPosition();
217
	void		InitializeVehicleIMU();
218
	void		InitializeVehicleMagnetometer();
219

220
	DEFINE_PARAMETERS(
221
		(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
222
		(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
223
224
		(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
225
	)
226
227
};

228
Sensors::Sensors(bool hil_enabled) :
229
	ModuleParams(nullptr),
230
	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
231
	_hil_enabled(hil_enabled),
Lorenz Meier's avatar
Lorenz Meier committed
232
	_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
233
	_voted_sensors_update(hil_enabled, _vehicle_imu_sub)
234
{
235
236
237
238
239
240
241
242
243
244
	/* Differential pressure offset */
	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
	_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */

	_parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
	_parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
	_parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");

245
	param_find("SYS_FAC_CAL_MODE");
246
247
248
249
250

	// Parameters controlling the on-board sensor thermal calibrator
	param_find("SYS_CAL_TDEL");
	param_find("SYS_CAL_TMAX");
	param_find("SYS_CAL_TMIN");
251
252
253

	_airspeed_validator.set_timeout(300000);
	_airspeed_validator.set_equal_value_threshold(100);
254

255
256
	_vehicle_acceleration.Start();
	_vehicle_angular_velocity.Start();
257
258
259
260
}

Sensors::~Sensors()
{
261
262
263
264
265
	// clear all registered callbacks
	for (auto &sub : _vehicle_imu_sub) {
		sub.unregisterCallback();
	}

266
267
	_vehicle_acceleration.Stop();
	_vehicle_angular_velocity.Stop();
268

269
270
271
272
273
	if (_vehicle_air_data) {
		_vehicle_air_data->Stop();
		delete _vehicle_air_data;
	}

274
275
276
277
278
	if (_vehicle_gps_position) {
		_vehicle_gps_position->Stop();
		delete _vehicle_gps_position;
	}

279
280
281
282
283
	if (_vehicle_magnetometer) {
		_vehicle_magnetometer->Stop();
		delete _vehicle_magnetometer;
	}

284
285
286
287
	for (auto &vehicle_imu : _vehicle_imu_list) {
		if (vehicle_imu) {
			vehicle_imu->Stop();
			delete vehicle_imu;
288
289
		}
	}
290
291

	perf_free(_loop_perf);
292
293
}

Daniel Agar's avatar
Daniel Agar committed
294
295
bool Sensors::init()
{
296
	_vehicle_imu_sub[0].registerCallback();
297
	ScheduleNow();
Daniel Agar's avatar
Daniel Agar committed
298
299
300
301
	return true;
}

int Sensors::parameters_update()
302
{
303
304
305
306
	if (_armed) {
		return 0;
	}

307
308
309
310
311
312
313
314
315
	/* Airspeed offset */
	param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
	param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */

	param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel);
	param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length);
	param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm);
Anton Babushkin's avatar
Anton Babushkin committed
316

317
	_voted_sensors_update.parametersUpdate();
318

319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
	// mark all existing sensor calibrations active even if sensor is missing
	// this preserves the calibration in the event of a parameter export while the sensor is missing
	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
		uint32_t device_id_accel = calibration::GetCalibrationParam("ACC",  "ID", i);
		uint32_t device_id_gyro  = calibration::GetCalibrationParam("GYRO", "ID", i);
		uint32_t device_id_mag   = calibration::GetCalibrationParam("MAG",  "ID", i);

		if (device_id_accel != 0) {
			bool external_accel = (calibration::GetCalibrationParam("ACC", "ROT", i) >= 0);
			calibration::Accelerometer accel_cal(device_id_accel, external_accel);
		}

		if (device_id_gyro != 0) {
			bool external_gyro = (calibration::GetCalibrationParam("GYRO", "ROT", i) >= 0);
			calibration::Gyroscope gyro_cal(device_id_gyro, external_gyro);
		}

		if (device_id_mag != 0) {
			bool external_mag = (calibration::GetCalibrationParam("MAG", "ROT", i) >= 0);
			calibration::Magnetometer mag_cal(device_id_mag, external_mag);
		}
	}

	// ensure calibration slots are active for the number of sensors currently available
	// this to done to eliminate differences in the active set of parameters before and after sensor calibration
	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
		if (orb_exists(ORB_ID(sensor_accel), i) == PX4_OK) {
			bool external = (calibration::GetCalibrationParam("ACC", "ROT", i) >= 0);
			calibration::Accelerometer cal{0, external};
			cal.set_calibration_index(i);
			cal.ParametersUpdate();
		}

		if (orb_exists(ORB_ID(sensor_gyro), i) == PX4_OK) {
			bool external = (calibration::GetCalibrationParam("GYRO", "ROT", i) >= 0);
			calibration::Gyroscope cal{0, external};
			cal.set_calibration_index(i);
			cal.ParametersUpdate();
		}

		if (orb_exists(ORB_ID(sensor_mag), i) == PX4_OK) {
			bool external = (calibration::GetCalibrationParam("MAG", "ROT", i) >= 0);
			calibration::Magnetometer cal{0, external};
			cal.set_calibration_index(i);
			cal.ParametersUpdate();
		}
	}

367
	return PX4_OK;
368
369
}

370
void Sensors::diff_pres_poll()
371
{
372
	differential_pressure_s diff_pres{};
373

374
	if (_diff_pres_sub.update(&diff_pres)) {
375

376
377
378
		vehicle_air_data_s air_data{};
		_vehicle_air_data_sub.copy(&air_data);

379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
		float air_temperature_celsius = NAN;

		// assume anything outside of a (generous) operating range of -40C to 125C is invalid
		if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) {

			air_temperature_celsius = diff_pres.temperature;

		} else {
			// differential pressure temperature invalid, check barometer
			if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius)
			    && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) {

				// TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro
				air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG;
			}
		}
395

Daniel Agar's avatar
Daniel Agar committed
396
		airspeed_s airspeed{};
397
		airspeed.timestamp = diff_pres.timestamp;
398

399
		/* push data into validator */
400
		float airspeed_input[3] = { diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.0f };
401

402
		_airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
Daniel Agar's avatar
Daniel Agar committed
403

404
		airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
405

406
407
		enum AIRSPEED_SENSOR_MODEL smodel;

408
		switch ((diff_pres.device_id >> 16) & 0xFF) {
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
		case DRV_DIFF_PRESS_DEVTYPE_SDP31:

		/* fallthrough */
		case DRV_DIFF_PRESS_DEVTYPE_SDP32:

		/* fallthrough */
		case DRV_DIFF_PRESS_DEVTYPE_SDP33:
			/* fallthrough */
			smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
			break;

		default:
			smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
			break;
		}

425
		/* don't risk to feed negative airspeed into the system */
426
		airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)
427
428
						  _parameters.air_cmodel,
						  smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
429
						  diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
430
431
						  air_temperature_celsius);

432
433
		airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
					     air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here
434

435
		airspeed.air_temperature_celsius = air_temperature_celsius;
436

437
		if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) {
438
			_airspeed_pub.publish(airspeed);
439
		}
440
441
442
	}
}

443
void
444
Sensors::parameter_update_poll(bool forced)
445
{
446
447
448
449
450
	// check for parameter updates
	if (_parameter_update_sub.updated() || forced) {
		// clear update
		parameter_update_s pupdate;
		_parameter_update_sub.copy(&pupdate);
451

452
		// update parameters from storage
453
		parameters_update();
454
		updateParams();
455

456
		/* update airspeed scale */
457
		int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
458
459

		/* this sensor is optional, abort without error */
460
		if (fd >= 0) {
461
462
463
464
465
			struct airspeed_scale airscale = {
				_parameters.diff_pres_offset_pa,
				1.0f,
			};

466
			if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
467
				warn("WARNING: failed to set scale / offsets for airspeed sensor");
468
469
			}

470
			px4_close(fd);
471
		}
472
	}
473
474
}

Daniel Agar's avatar
Daniel Agar committed
475
void Sensors::adc_poll()
px4dev's avatar
px4dev committed
476
{
477
478
	/* only read if not in HIL mode */
	if (_hil_enabled) {
Lorenz Meier's avatar
Lorenz Meier committed
479
		return;
480
	}
px4dev's avatar
px4dev committed
481

482
483
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL

484
	if (_parameters.diff_pres_analog_scale > 0.0f) {
485

486
		hrt_abstime t = hrt_absolute_time();
487

488
489
		/* rate limit to 100 Hz */
		if (t - _last_adc >= 10000) {
490
			adc_report_s adc;
Lorenz Meier's avatar
Lorenz Meier committed
491

492
			if (_adc_report_sub.update(&adc)) {
493
				/* Read add channels we got */
494
495
496
497
498
499
				for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) {
					if (adc.channel_id[i] == -1) {
						continue;	// skip non-exist channels
					}

					if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) {
500

501
						/* calculate airspeed, raw is the difference from */
502
						const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV;
503

504
505
506
507
						/**
						 * The voltage divider pulls the signal down, only act on
						 * a valid voltage from a connected sensor. Also assume a non-
						 * zero offset from the sensor if its connected.
508
509
510
						 *
						 * Notice: This won't work on devices which have PGA controlled
						 * vref. Those devices require no divider at all.
511
						 */
512
513
						if (voltage > 0.4f) {
							const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
514

515
516
517
518
519
							_diff_pres.timestamp = t;
							_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
							_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) +
									(diff_pres_pa_raw * 0.1f);
							_diff_pres.temperature = -1000.0f;
520

521
							_diff_pres_pub.publish(_diff_pres);
522
523
						}
					}
524
				}
525
			}
526
527

			_last_adc = t;
px4dev's avatar
px4dev committed
528
529
530
		}
	}

531
532
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
}
533

534
535
536
537
void Sensors::InitializeVehicleAirData()
{
	if (_param_sys_has_baro.get()) {
		if (_vehicle_air_data == nullptr) {
538
			_vehicle_air_data = new VehicleAirData();
539

540
541
			if (_vehicle_air_data) {
				_vehicle_air_data->Start();
542
543
544
545
546
			}
		}
	}
}

547
548
void Sensors::InitializeVehicleGPSPosition()
{
549
550
	if (_param_sys_has_gps.get()) {
		if (_vehicle_gps_position == nullptr) {
551
552
553
554
555
556
557
558
559
			_vehicle_gps_position = new VehicleGPSPosition();

			if (_vehicle_gps_position) {
				_vehicle_gps_position->Start();
			}
		}
	}
}

560
561
562
563
564
565
void Sensors::InitializeVehicleIMU()
{
	// create a VehicleIMU instance for each accel/gyro pair
	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
		if (_vehicle_imu_list[i] == nullptr) {

566
567
			uORB::Subscription accel_sub{ORB_ID(sensor_accel), i};
			sensor_accel_s accel{};
568
569
			accel_sub.copy(&accel);

570
571
			uORB::Subscription gyro_sub{ORB_ID(sensor_gyro), i};
			sensor_gyro_s gyro{};
572
573
574
			gyro_sub.copy(&gyro);

			if (accel.device_id > 0 && gyro.device_id > 0) {
575
576
				// if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ
				//   otherwise each VehicleIMU runs in a corresponding INSx WQ
Daniel Agar's avatar
Daniel Agar committed
577
578
				const bool multi_mode = (_param_sens_imu_mode.get() == 0);
				const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(i) : px4::wq_configurations::INS0;
579
580

				VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config);
581
582
583
584
585
586
587
588
589
590

				if (imu != nullptr) {
					// Start VehicleIMU instance and store
					if (imu->Start()) {
						_vehicle_imu_list[i] = imu;

					} else {
						delete imu;
					}
				}
591
592
593
594

			} else {
				// abort on first failure, try again later
				return;
595
596
597
598
599
			}
		}
	}
}

600
601
602
603
void Sensors::InitializeVehicleMagnetometer()
{
	if (_param_sys_has_mag.get()) {
		if (_vehicle_magnetometer == nullptr) {
604
			_vehicle_magnetometer = new VehicleMagnetometer();
605

606
607
			if (_vehicle_magnetometer) {
				_vehicle_magnetometer->Start();
608
609
610
611
612
			}
		}
	}
}

Daniel Agar's avatar
Daniel Agar committed
613
void Sensors::Run()
614
{
Daniel Agar's avatar
Daniel Agar committed
615
616
	if (should_exit()) {
		// clear all registered callbacks
617
		for (auto &sub : _vehicle_imu_sub) {
Daniel Agar's avatar
Daniel Agar committed
618
619
			sub.unregisterCallback();
		}
Thomas Gubler's avatar
Thomas Gubler committed
620

Daniel Agar's avatar
Daniel Agar committed
621
622
623
		exit_and_cleanup();
		return;
	}
624

Daniel Agar's avatar
Daniel Agar committed
625
626
	// run once
	if (_last_config_update == 0) {
627
628
		InitializeVehicleAirData();
		InitializeVehicleIMU();
629
		InitializeVehicleGPSPosition();
630
		InitializeVehicleMagnetometer();
Daniel Agar's avatar
Daniel Agar committed
631
632
633
		_voted_sensors_update.init(_sensor_combined);
		parameter_update_poll(true);
	}
634

Daniel Agar's avatar
Daniel Agar committed
635
	perf_begin(_loop_perf);
636

Daniel Agar's avatar
Daniel Agar committed
637
638
	// backup schedule as a watchdog timeout
	ScheduleDelayed(10_ms);
639

Daniel Agar's avatar
Daniel Agar committed
640
641
642
	// check vehicle status for changes to publication state
	if (_vcontrol_mode_sub.updated()) {
		vehicle_control_mode_s vcontrol_mode{};
643

Daniel Agar's avatar
Daniel Agar committed
644
645
646
647
		if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
			_armed = vcontrol_mode.flag_armed;
		}
	}
648

649
	_voted_sensors_update.sensorsPoll(_sensor_combined);
650

Daniel Agar's avatar
Daniel Agar committed
651
652
	// check analog airspeed
	adc_poll();
653

654
	diff_pres_poll();
655

Daniel Agar's avatar
Daniel Agar committed
656
	if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
657

Daniel Agar's avatar
Daniel Agar committed
658
659
660
661
		_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
		_sensor_pub.publish(_sensor_combined);
		_sensor_combined_prev_timestamp = _sensor_combined.timestamp;
	}
662

Daniel Agar's avatar
Daniel Agar committed
663
664
	// keep adding sensors as long as we are not armed,
	// when not adding sensors poll for param updates
665
	if (!_armed && hrt_elapsed_time(&_last_config_update) > 1000_ms) {
666
667
668
669
670
671
672
673
674
675
676
677
678

		const int n_accel = orb_group_count(ORB_ID(sensor_accel));
		const int n_baro  = orb_group_count(ORB_ID(sensor_baro));
		const int n_gps   = orb_group_count(ORB_ID(sensor_gps));
		const int n_gyro  = orb_group_count(ORB_ID(sensor_gyro));
		const int n_mag   = orb_group_count(ORB_ID(sensor_mag));

		if ((n_accel != _n_accel) || (n_baro != _n_baro) || (n_gps != _n_gps) || (n_gyro != _n_gyro) || (n_mag != _n_mag)) {
			_n_accel = n_accel;
			_n_baro = n_baro;
			_n_gps = n_gps;
			_n_gyro = n_gyro;
			_n_mag = n_mag;
679

680
681
682
683
684
685
686
			parameters_update();

			InitializeVehicleAirData();
			InitializeVehicleGPSPosition();
			InitializeVehicleMagnetometer();
		}

687
688
689
690
		// sensor device id (not just orb_group_count) must be populated before IMU init can succeed
		_voted_sensors_update.initializeSensors();
		InitializeVehicleIMU();

Daniel Agar's avatar
Daniel Agar committed
691
		_last_config_update = hrt_absolute_time();
692

Daniel Agar's avatar
Daniel Agar committed
693
694
695
696
	} else {
		// check parameters for updates
		parameter_update_poll();
	}
697

Daniel Agar's avatar
Daniel Agar committed
698
699
	perf_end(_loop_perf);
}
700

Daniel Agar's avatar
Daniel Agar committed
701
702
703
704
int Sensors::task_spawn(int argc, char *argv[])
{
	bool hil_enabled = false;
	bool error_flag = false;
705

Daniel Agar's avatar
Daniel Agar committed
706
707
708
	int myoptind = 1;
	int ch;
	const char *myoptarg = nullptr;
709

Daniel Agar's avatar
Daniel Agar committed
710
711
712
713
714
	while ((ch = px4_getopt(argc, argv, "h", &myoptind, &myoptarg)) != EOF) {
		switch (ch) {
		case 'h':
			hil_enabled = true;
			break;
715

Daniel Agar's avatar
Daniel Agar committed
716
717
718
		case '?':
			error_flag = true;
			break;
719

Daniel Agar's avatar
Daniel Agar committed
720
721
722
723
		default:
			PX4_WARN("unrecognized flag");
			error_flag = true;
			break;
724
		}
Daniel Agar's avatar
Daniel Agar committed
725
	}
726

Daniel Agar's avatar
Daniel Agar committed
727
728
	if (error_flag) {
		return PX4_ERROR;
729
730
	}

Daniel Agar's avatar
Daniel Agar committed
731
	Sensors *instance = new Sensors(hil_enabled);
732

Daniel Agar's avatar
Daniel Agar committed
733
734
735
736
737
738
739
740
741
742
	if (instance) {
		_object.store(instance);
		_task_id = task_id_is_work_queue;

		if (instance->init()) {
			return PX4_OK;
		}

	} else {
		PX4_ERR("alloc failed");
743
744
	}

Daniel Agar's avatar
Daniel Agar committed
745
746
747
748
749
	delete instance;
	_object.store(nullptr);
	_task_id = -1;

	return PX4_ERROR;
750
751
}

752
int Sensors::print_status()
753
{
754
	_voted_sensors_update.printStatus();
Beat Küng's avatar
Beat Küng committed
755

756
757
758
759
760
	if (_vehicle_magnetometer) {
		PX4_INFO_RAW("\n");
		_vehicle_magnetometer->PrintStatus();
	}

761
762
763
764
	if (_vehicle_air_data) {
		PX4_INFO_RAW("\n");
		_vehicle_air_data->PrintStatus();
	}
765

766
	PX4_INFO_RAW("\n");
Beat Küng's avatar
Beat Küng committed
767
	PX4_INFO("Airspeed status:");
768
	_airspeed_validator.print();
769

770
	PX4_INFO_RAW("\n");
771
	_vehicle_acceleration.PrintStatus();
772
773

	PX4_INFO_RAW("\n");
774
	_vehicle_angular_velocity.PrintStatus();
775

776
777
778
779
780
	if (_vehicle_gps_position) {
		PX4_INFO_RAW("\n");
		_vehicle_gps_position->PrintStatus();
	}

781
782
	PX4_INFO_RAW("\n");

783
784
	for (auto &i : _vehicle_imu_list) {
		if (i != nullptr) {
785
			PX4_INFO_RAW("\n");
786
787
788
789
			i->PrintStatus();
		}
	}

790
	return 0;
791
792
}

793
794
795
796
int Sensors::custom_command(int argc, char *argv[])
{
	return print_usage("unknown command");
}
797

798
int Sensors::print_usage(const char *reason)
799
{
800
801
	if (reason) {
		PX4_WARN("%s\n", reason);
802
	}
803

804
805
806
807
808
809
810
811
812
813
814
	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
The sensors module is central to the whole system. It takes low-level output from drivers, turns
it into a more usable form, and publishes it for the rest of the system.

The provided functionality includes:
- Read the output from the sensor drivers (`sensor_gyro`, etc.).
  If there are multiple of the same type, do voting and failover handling.
  Then apply the board rotation and temperature calibration (if enabled). And finally publish the data; one of the
  topics is `sensor_combined`, used by many parts of the system.
Beat Küng's avatar
Beat Küng committed
815
816
817
- Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or
  on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the
  sensor drivers must already be running when `sensors` is started.
818
- Do sensor consistency checks and publish the `sensors_status_imu` topic.
819
820
821
822
823
824
825
826
827
828
829
830
831

### Implementation
It runs in its own thread and polls on the currently selected gyro topic.

)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("sensors", "system");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_PARAM_FLAG('h', "Start in HIL mode", true);
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

	return 0;
}
832

833
extern "C" __EXPORT int sensors_main(int argc, char *argv[])
834
835
{
	return Sensors::main(argc, argv);
836
}