PX4 Firmware
PX4 Autopilot Software http://px4.io
airspeed_selector_main.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #include <drivers/drv_hrt.h>
36 #include <matrix/math.hpp>
37 #include <parameters/param.h>
38 #include <perf/perf_counter.h>
39 #include <px4_platform_common/module.h>
40 #include <px4_platform_common/module_params.h>
41 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
42 #include <lib/airspeed/airspeed.h>
43 #include <AirspeedValidator.hpp>
44 #include <systemlib/mavlink_log.h>
45 
46 #include <uORB/Subscription.hpp>
47 #include <uORB/topics/airspeed.h>
52 #include <uORB/Publication.hpp>
62 
63 using namespace time_literals;
64 
65 static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */
66 
67 using matrix::Dcmf;
68 using matrix::Quatf;
69 using matrix::Vector2f;
70 using matrix::Vector3f;
71 
72 class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
73  public px4::ScheduledWorkItem
74 {
75 public:
76 
78 
79  ~AirspeedModule() override;
80 
81  /** @see ModuleBase */
82  static int task_spawn(int argc, char *argv[]);
83 
84  /** @see ModuleBase */
85  static int custom_command(int argc, char *argv[]);
86 
87  /** @see ModuleBase */
88  static int print_usage(const char *reason = nullptr);
89 
90  /* run the main loop */
91  void Run() override;
92 
93  int print_status() override;
94 
95 private:
96  static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */
98  DISABLED_INDEX = -1,
102  THIRD_SENSOR_INDEX
103  };
104 
105  uORB::Publication<airspeed_validated_s> _airspeed_validated_pub {ORB_ID(airspeed_validated)}; /**< airspeed validated topic*/
106  uORB::PublicationMulti<wind_estimate_s> _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
107  orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/
108 
109  uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
111  uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
112  uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
113  uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
114  uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
115  uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
116  uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
117  uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
118  uORB::Subscription _airspeed_sub[MAX_NUM_AIRSPEED_SENSORS] {{ORB_ID(airspeed), 0}, {ORB_ID(airspeed), 1}, {ORB_ID(airspeed), 2}}; /**< raw airspeed topics subscriptions. */
119 
120  estimator_status_s _estimator_status {};
122  vehicle_air_data_s _vehicle_air_data {};
123  vehicle_attitude_s _vehicle_attitude {};
124  vehicle_land_detected_s _vehicle_land_detected {};
125  vehicle_local_position_s _vehicle_local_position {};
126  vehicle_status_s _vehicle_status {};
127  vtol_vehicle_status_s _vtol_vehicle_status {};
128 
129  WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */
130  wind_estimate_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */
131 
132  int32_t _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/
133  int32_t _prev_number_of_airspeed_sensors{0}; /**< number of airspeed sensors in previous loop (to detect a new added sensor)*/
134  AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS] {}; /**< airspeedValidator instances (one for each sensor) */
135 
136  hrt_abstime _time_now_usec{0};
137  int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */
138  int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */
139  bool _initialized{false}; /**< module initialized*/
140  bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
141  bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
142  float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
143  float _ground_minus_wind_EAS{0.0f}; /**< equivalent airspeed from groundspeed minus windspeed */
144 
145  bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
146 
147  perf_counter_t _perf_elapsed{};
148 
149  DEFINE_PARAMETERS(
150  (ParamFloat<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
151  (ParamFloat<px4::params::ASPD_SC_P_NOISE>) _param_west_sc_p_noise,
152  (ParamFloat<px4::params::ASPD_TAS_NOISE>) _param_west_tas_noise,
153  (ParamFloat<px4::params::ASPD_BETA_NOISE>) _param_west_beta_noise,
154  (ParamInt<px4::params::ASPD_TAS_GATE>) _param_west_tas_gate,
155  (ParamInt<px4::params::ASPD_BETA_GATE>) _param_west_beta_gate,
156  (ParamInt<px4::params::ASPD_SCALE_EST>) _param_west_scale_estimation_on,
157  (ParamFloat<px4::params::ASPD_SCALE>) _param_west_airspeed_scale,
158  (ParamInt<px4::params::ASPD_PRIMARY>) _param_airspeed_primary_index,
159  (ParamInt<px4::params::ASPD_DO_CHECKS>) _param_airspeed_checks_on,
160  (ParamInt<px4::params::ASPD_FALLBACK>) _param_airspeed_fallback,
161 
162  (ParamFloat<px4::params::ASPD_FS_INNOV>) _tas_innov_threshold, /**< innovation check threshold */
163  (ParamFloat<px4::params::ASPD_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
164  (ParamInt<px4::params::ASPD_FS_T1>) _checks_fail_delay, /**< delay to declare airspeed invalid */
165  (ParamInt<px4::params::ASPD_FS_T2>) _checks_clear_delay, /**< delay to declare airspeed valid again */
166  (ParamFloat<px4::params::ASPD_STALL>) _airspeed_stall /**< stall speed*/
167  )
168 
169  void init(); /**< initialization of the airspeed validator instances */
170  void check_for_connected_airspeed_sensors(); /**< check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors */
171  void update_params(); /**< update parameters */
172  void poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */
173  void update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */
174  void update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */
175  void select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */
176 
177 };
178 
180  ModuleParams(nullptr),
181  ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
182 {
183  // initialise parameters
184  update_params();
185 
186  _perf_elapsed = perf_alloc_once(PC_ELAPSED, "airspeed_selector elapsed");
187 }
188 
190 {
191  ScheduleClear();
192 
194 
195 }
196 
197 int
198 AirspeedModule::task_spawn(int argc, char *argv[])
199 {
200  AirspeedModule *dev = new AirspeedModule();
201 
202  // check if the trampoline is called for the first time
203  if (!dev) {
204  PX4_ERR("alloc failed");
205  return PX4_ERROR;
206  }
207 
208  _object.store(dev);
209 
210  dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000);
211  _task_id = task_id_is_work_queue;
212  return PX4_OK;
213 
214 }
215 void
217 {
219 
220  /* Set the default sensor */
221  if (_param_airspeed_primary_index.get() > _number_of_airspeed_sensors) {
222  /* constrain the index to the number of sensors connected*/
223  _valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors);
224 
225  if (_number_of_airspeed_sensors == 0) {
227  "No airspeed sensor detected. Switch to non-airspeed mode.");
228 
229  } else {
231  "Primary airspeed index bigger than number connected sensors. Take last sensor.");
232  }
233 
234  } else {
236  _param_airspeed_primary_index.get(); // set index to the one provided in the parameter ASPD_PRIMARY
237  }
238 
239  _prev_airspeed_index = _valid_airspeed_index; // needed to detect a switching
240 }
241 
242 void
244 {
245  /* check for new connected airspeed sensor */
246  int detected_airspeed_sensors = 0;
247 
248  if (_param_airspeed_primary_index.get() > 0) {
249 
250  for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
251  if (!_airspeed_sub[i].advertised()) {
252  break;
253  }
254 
255  detected_airspeed_sensors = i + 1;
256  }
257 
258  } else {
259  detected_airspeed_sensors = 0; //user has selected groundspeed-windspeed as primary source, or disabled airspeed
260  }
261 
262  _number_of_airspeed_sensors = detected_airspeed_sensors;
263 }
264 
265 
266 void
268 {
270 
271  if (!_initialized) {
272  init(); // initialize airspeed validator instances
273  _initialized = true;
274  }
275 
276  parameter_update_s update;
277 
278  if (_param_sub.update(&update)) {
279  update_params();
280  }
281 
282  _time_now_usec = hrt_absolute_time(); //hrt time of the current cycle
283 
284  bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
285 
286  /* Check for new connected airspeed sensors as long as we're disarmed */
287  if (!armed) {
289  }
290 
291  poll_topics();
294 
295  if (_number_of_airspeed_sensors > 0) {
296 
297  bool fixed_wing = !_vtol_vehicle_status.vtol_in_rw_mode;
298  bool in_air = !_vehicle_land_detected.landed;
299 
300  /* Prepare data for airspeed_validator */
301  struct airspeed_validator_update_data input_data = {};
302  input_data.timestamp = _time_now_usec;
303  input_data.lpos_vx = _vehicle_local_position.vx;
304  input_data.lpos_vy = _vehicle_local_position.vy;
305  input_data.lpos_vz = _vehicle_local_position.vz;
307  input_data.lpos_evh = _vehicle_local_position.evh;
308  input_data.lpos_evv = _vehicle_local_position.evv;
309  input_data.att_q[0] = _vehicle_attitude.q[0];
310  input_data.att_q[1] = _vehicle_attitude.q[1];
311  input_data.att_q[2] = _vehicle_attitude.q[2];
312  input_data.att_q[3] = _vehicle_attitude.q[3];
314  input_data.accel_z = _accel.xyz[2];
317 
318  /* iterate through all airspeed sensors, poll new data from them and update their validators */
319  for (int i = 0; i < _number_of_airspeed_sensors; i++) {
320 
321  /* poll airspeed data */
322  airspeed_s airspeed_raw = {};
323  _airspeed_sub[i].copy(&airspeed_raw); // poll raw airspeed topic of the i-th sensor
324  input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s;
325  input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s;
326  input_data.airspeed_timestamp = airspeed_raw.timestamp;
327  input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius;
328 
329  /* update in_fixed_wing_flight for the current airspeed sensor validator */
330  /* takeoff situation is active from start till one of the sensors' IAS or groundspeed_EAS is above stall speed */
331  if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_EAS > _airspeed_stall.get()) {
332  _in_takeoff_situation = false;
333  }
334 
335  /* reset takeoff_situation to true when not in air or not in fixed-wing mode */
336  if (!in_air || !fixed_wing) {
337  _in_takeoff_situation = true;
338  }
339 
340  input_data.in_fixed_wing_flight = (armed && fixed_wing && in_air && !_in_takeoff_situation);
341 
342  /* push input data into airspeed validator */
344 
345  }
346  }
347 
349 
351 
352  if (should_exit()) {
353  exit_and_cleanup();
354  }
355 }
356 
358 {
359  updateParams();
360 
361  /* update wind estimator (sideslip fusion only) parameters */
362  _wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get());
363  _wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
364  _wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
365  _wind_estimator_sideslip.set_beta_noise(_param_west_beta_noise.get());
366  _wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
367  _wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());
368 
369  /* update airspeedValidator parameters */
370  for (int i = 0; i < _number_of_airspeed_sensors; i++) {
371  _airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
372  _airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get());
373  _airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get());
374  _airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get());
375  _airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get());
376  _airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get());
377  _airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get());
378 
379  /* Only apply manual entered airspeed scale to first airspeed measurement */
380  // TODO: enable multiple airspeed sensors
381  _airspeed_validator[0].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
382 
383  _airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get());
384  _airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get());
385  _airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get());
386  _airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get());
387  _airspeed_validator[i].set_airspeed_stall(_airspeed_stall.get());
388  }
389 
390  /* when airspeed scale estimation is turned on and the airspeed is valid, then set the scale inside the wind estimator to -1 such that it starts to estimate it */
391  if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) {
392  if (_valid_airspeed_index > 0) {
394  -1.0f); // set it to a negative value to start estimation inside wind estimator
395 
396  } else {
397  mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
398  _param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on
399  _param_west_scale_estimation_on.commit_no_notification();
400  }
401 
402  /* If one sensor is valid and we switched out of scale estimation, then publish message and change the value of param ASPD_ASPD_SCALE */
403 
404  } else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
405  if (_valid_airspeed_index > 0) {
406 
407  _param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_EAS_scale());
408  _param_west_airspeed_scale.commit_no_notification();
409  _airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
410 
411  mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_ASPD_SCALE): %0.2f",
412  (double)_airspeed_validator[_valid_airspeed_index - 1].get_EAS_scale());
413 
414  } else {
415  mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
416  }
417  }
418 
419  _scale_estimation_previously_on = _param_west_scale_estimation_on.get();
420 
421 }
422 
424 {
433 
436 }
437 
439 {
440  bool att_valid = true; // TODO: check if attitude is valid
441 
442  /* update wind and airspeed estimator */
444 
445  if (_vehicle_local_position_valid && att_valid) {
448 
449  /* sideslip fusion */
451  }
452 
453  /* fill message for publishing later */
455  float wind[2];
459  float wind_cov[2];
468 }
469 
471 {
472  /* calculate airspeed estimate based on groundspeed-windspeed to use as fallback */
475  float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
476  _ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
479 }
480 
481 
483 {
484  /* Find new valid index if airspeed currently is invalid, but we have sensors, primary sensor is real sensor and checks are enabled or new sensor was added. */
485  bool airspeed_sensor_switching_necessary = _prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX ||
487  bool airspeed_sensor_switching_allowed = _number_of_airspeed_sensors > 0 &&
488  _param_airspeed_primary_index.get() > airspeed_index::GROUND_MINUS_WIND_INDEX && _param_airspeed_checks_on.get();
489  bool airspeed_sensor_added = _prev_number_of_airspeed_sensors < _number_of_airspeed_sensors;
490 
491  if (airspeed_sensor_switching_necessary && (airspeed_sensor_switching_allowed || airspeed_sensor_added)) {
492 
493  _valid_airspeed_index = airspeed_index::DISABLED_INDEX; // set to disabled
494 
495  /* Loop through all sensors and take the first valid one */
496  for (int i = 0; i < _number_of_airspeed_sensors; i++) {
497  if (_airspeed_validator[i].get_airspeed_valid()) {
498  _valid_airspeed_index = i + 1;
499  break;
500  }
501  }
502  }
503 
504  /* Airspeed enabled by user (Primary set to > -1), and no valid airspeed sensor available or primary set to 0. Thus set index to ground-wind one (if position is valid), otherwise to disabled*/
505  if (_param_airspeed_primary_index.get() > airspeed_index::DISABLED_INDEX &&
506  (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
507  || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
508 
509  /* _vehicle_local_position_valid determines if ground-wind estimate is valid */
510  /* To use ground-windspeed as airspeed source, either the primary has to be set this way or fallback be enabled */
511  if (_vehicle_local_position_valid && (_param_airspeed_fallback.get()
512  || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
513  _valid_airspeed_index = airspeed_index::GROUND_MINUS_WIND_INDEX;
514 
515  } else {
516  _valid_airspeed_index = airspeed_index::DISABLED_INDEX;
517  }
518  }
519 
520  /* publish critical message (and log) in index has changed */
521  /* Suppress log message if still on the ground and no airspeed sensor connected */
522  if (_valid_airspeed_index != _prev_airspeed_index && (_number_of_airspeed_sensors > 0
524  mavlink_log_critical(&_mavlink_log_pub, "Airspeed: switched from sensor %i to %i", _prev_airspeed_index,
526  }
527 
530 
531  /* fill out airspeed_validated message for publishing it */
532  airspeed_validated_s airspeed_validated = {};
533  airspeed_validated.timestamp = _time_now_usec;
534  airspeed_validated.true_ground_minus_wind_m_s = NAN;
535  airspeed_validated.equivalent_ground_minus_wind_m_s = NAN;
536  airspeed_validated.indicated_airspeed_m_s = NAN;
537  airspeed_validated.equivalent_airspeed_m_s = NAN;
538  airspeed_validated.true_airspeed_m_s = NAN;
539  airspeed_validated.airspeed_sensor_measurement_valid = false;
540  airspeed_validated.selected_airspeed_index = _valid_airspeed_index;
541 
542  switch (_valid_airspeed_index) {
543  case airspeed_index::DISABLED_INDEX:
544  break;
545 
546  case airspeed_index::GROUND_MINUS_WIND_INDEX:
547  /* Take IAS, EAS, TAS from groundspeed-windspeed */
548  airspeed_validated.indicated_airspeed_m_s = _ground_minus_wind_EAS;
549  airspeed_validated.equivalent_airspeed_m_s = _ground_minus_wind_EAS;
550  airspeed_validated.true_airspeed_m_s = _ground_minus_wind_TAS;
553 
554  break;
555 
556  default:
562  airspeed_validated.airspeed_sensor_measurement_valid = true;
563  break;
564  }
565 
566  /* publish airspeed validated topic */
567  _airspeed_validated_pub.publish(airspeed_validated);
568 
569  /* publish sideslip-only-fusion wind topic */
571 
572  /* publish the wind estimator states from all airspeed validators */
573  for (int i = 0; i < _number_of_airspeed_sensors; i++) {
575  _wind_est_pub[i + 1].publish(wind_est);
576  }
577 
578 }
579 
580 int AirspeedModule::custom_command(int argc, char *argv[])
581 {
582  if (!is_running()) {
583  int ret = AirspeedModule::task_spawn(argc, argv);
584 
585  if (ret) {
586  return ret;
587  }
588  }
589 
590  return print_usage("unknown command");
591 }
592 
594 {
596 
597  int instance = 0;
598  uORB::SubscriptionData<airspeed_validated_s> est{ORB_ID(airspeed_validated), (uint8_t)instance};
599  est.update();
600  PX4_INFO("Number of airspeed sensors: %i", _number_of_airspeed_sensors);
601  print_message(est.get());
602 
603  return 0;
604 }
605 
606 int AirspeedModule::print_usage(const char *reason)
607 {
608  if (reason) {
609  PX4_WARN("%s\n", reason);
610  }
611 
612  PRINT_MODULE_DESCRIPTION(
613  R"DESCR_STR(
614 ### Description
615 This module provides a single airspeed_validated topic, containing an indicated (IAS),
616 equivalent (EAS), true airspeed (TAS) and the information if the estimation currently
617 is invalid and if based sensor readings or on groundspeed minus windspeed.
618 Supporting the input of multiple "raw" airspeed inputs, this module automatically switches
619 to a valid sensor in case of failure detection. For failure detection as well as for
620 the estimation of a scale factor from IAS to EAS, it runs several wind estimators
621 and also publishes those.
622 
623 )DESCR_STR");
624 
625  PRINT_MODULE_USAGE_NAME("airspeed_estimator", "estimator");
626  PRINT_MODULE_USAGE_COMMAND("start");
627  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
628 
629  return 0;
630 }
631 
632 extern "C" __EXPORT int airspeed_selector_main(int argc, char *argv[])
633 {
634  return AirspeedModule::main(argc, argv);
635 }
void set_wind_estimator_beta_noise(float beta_var)
void check_for_connected_airspeed_sensors()
check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors
void set_wind_estimator_tas_scale_p_noise(float tas_scale_sigma)
WindEstimator _wind_estimator_sideslip
wind estimator instance only fusing sideslip
void set_wind_estimator_tas_noise(float tas_sigma)
void update(uint64_t time_now)
vehicle_acceleration_s _accel
measure the time elapsed performing an event
Definition: perf_counter.h:56
wind_estimate_s _wind_estimate_sideslip
wind estimate message for wind estimator instance only fusing sideslip
Dcm< float > Dcmf
Definition: Dcm.hpp:185
uORB::Publication< airspeed_validated_s > _airspeed_validated_pub
airspeed validated topic
void get_wind(float wind[2])
void set_wind_estimator_wind_p_noise(float wind_sigma)
float true_airspeed_m_s
Definition: airspeed.h:55
int main(int argc, char **argv)
Definition: main.cpp:3
uORB::Subscription _vtol_vehicle_status_sub
float calc_EAS_from_TAS(float speed_true, float pressure_ambient, float temperature_celsius)
Calculate equivalent airspeed (EAS) from true airspeed (TAS).
Definition: airspeed.cpp:282
int32_t _number_of_airspeed_sensors
number of airspeed sensors in use (detected during initialization)
float air_temperature_celsius
Definition: airspeed.h:56
Definition: I2C.hpp:51
void set_checks_fail_delay(float checks_fail_delay)
void print_status()
Definition: Commander.cpp:517
int print_status() override
int32_t _prev_number_of_airspeed_sensors
number of airspeed sensors in previous loop (to detect a new added sensor)
perf_counter_t _perf_elapsed
void set_wind_estimator_tas_gate(uint8_t gate_size)
static void print_usage()
static bool is_running()
Definition: dataman.cpp:415
LidarLite * instance
Definition: ll40ls.cpp:65
void update_params()
update parameters
void get_wind_var(float wind_var[2])
High-resolution timer with callouts and timekeeping.
uORB::Subscription _vehicle_air_data_sub
uORB::Subscription _vehicle_land_detected_sub
float get_beta_innov_var()
float get_beta_innov()
Global flash based parameter store.
void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att)
void select_airspeed_and_publish()
select airspeed sensor (or groundspeed-windspeed)
A wind and airspeed scale estimator.
int _valid_airspeed_index
index of currently chosen (valid) airspeed sensor
Calculates airspeed from differential pressure and checks if this airspeed is valid.
void set_airspeed_stall(float airspeed_stall)
uORB::Subscription _vehicle_attitude_sub
bool _initialized
module initialized
void set_tas_scale_p_noise(float tas_scale_sigma)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
float _ground_minus_wind_TAS
true airspeed from groundspeed minus windspeed
void set_beta_noise(float beta_var)
wind_estimate_s get_wind_estimator_states(uint64_t timestamp)
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
uint64_t timestamp
Definition: wind_estimate.h:53
void poll_topics()
poll all topics required beside airspeed (e.g.
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
static sensor_accel_s _accel
accel report
bool _vehicle_local_position_valid
local position (from GPS) valid
orb_advert_t _mavlink_log_pub
mavlink log topic
void set_checks_clear_delay(float checks_clear_delay)
void update_airspeed_validator(const airspeed_validator_update_data &input_data)
void set_beta_gate(uint8_t gate_size)
void set_tas_noise(float tas_sigma)
perf_counter_t perf_alloc_once(enum perf_counter_type type, const char *name)
Get the reference to an existing counter or create a new one if it does not exist.
static struct actuator_armed_s armed
Definition: Commander.cpp:139
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Vector2< float > Vector2f
Definition: Vector2.hpp:69
float get_tas_scale()
float indicated_airspeed_m_s
Definition: airspeed.h:54
vehicle_status_s _vehicle_status
vtol_vehicle_status_s _vtol_vehicle_status
void set_airspeed_scale_manual(float airspeed_scale_manual)
void perf_end(perf_counter_t handle)
End a performance event.
vehicle_attitude_s _vehicle_attitude
void update_wind_estimator_sideslip()
update the wind estimator instance only fusing sideslip
__EXPORT int airspeed_selector_main(int argc, char *argv[])
uORB::Subscription _airspeed_sub[MAX_NUM_AIRSPEED_SENSORS]
raw airspeed topics subscriptions.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
static constexpr int MAX_NUM_AIRSPEED_SENSORS
Support max 3 airspeed sensors.
static void update_params(ParameterHandles &param_handles, Parameters &params, bool &got_changes)
Definition: vmount.cpp:525
Vector3< float > Vector3f
Definition: Vector3.hpp:136
AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS]
airspeedValidator instances (one for each sensor)
int _prev_airspeed_index
previously chosen airspeed sensor index
float get_tas_innov()
uORB::Subscription _vehicle_local_position_sub
static int print_usage(const char *reason=nullptr)
uint64_t timestamp
Definition: airspeed.h:53
bool _in_takeoff_situation
in takeoff situation (defined as not yet stall speed reached)
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
uORB::Subscription _estimator_status_sub
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
vehicle_land_detected_s _vehicle_land_detected
static int task_spawn(int argc, char *argv[])
void set_tas_innov_threshold(float tas_innov_threshold)
vehicle_local_position_s _vehicle_local_position
Definition: bst.cpp:62
void set_tas_innov_integ_threshold(float tas_innov_integ_threshold)
uORB::PublicationMulti< wind_estimate_s > _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS+1]
wind estimate topic (for each airspeed validator + purely sideslip fusion)
float _ground_minus_wind_EAS
equivalent airspeed from groundspeed minus windspeed
bool update(void *dst)
Update the struct.
void set_tas_gate(uint8_t gate_size)
float get_tas_innov_var()
void set_wind_p_noise(float wind_sigma)
bool _scale_estimation_previously_on
scale_estimation was on in the last cycle
bool publish(const T &data)
Publish the struct.
bool copy(void *dst)
Copy the struct.
vehicle_air_data_s _vehicle_air_data
estimator_status_s _estimator_status
void update_ground_minus_wind_airspeed()
update airspeed estimate based on groundspeed minus windspeed
void set_wind_estimator_scale_estimation_on(bool scale_estimation_on)
void perf_begin(perf_counter_t handle)
Begin a performance event.
static int custom_command(int argc, char *argv[])
uORB::Subscription _param_sub
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uORB::Subscription _vehicle_acceleration_sub
uORB::Subscription _vehicle_status_sub
void set_wind_estimator_beta_gate(uint8_t gate_size)
Performance measuring tools.
static int _airspeed_sub
Definition: messages.cpp:64
static constexpr uint32_t SCHEDULE_INTERVAL
The schedule interval in usec (10 Hz)