2013-08-29 02:34:34 -03:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
2012-07-15 22:21:20 -03:00
/*
2018-07-23 20:47:40 -03:00
* AP_Airspeed . cpp - airspeed ( pitot ) driver
2012-08-17 03:08:43 -03:00
*/
2012-07-15 22:21:20 -03:00
2016-01-29 07:38:33 -04:00
# include <AP_Common/AP_Common.h>
2015-08-11 03:28:42 -03:00
# include <AP_HAL/AP_HAL.h>
2016-06-08 09:13:53 -03:00
# include <AP_HAL/I2CDevice.h>
2015-08-11 03:28:42 -03:00
# include <AP_Math/AP_Math.h>
2016-05-24 01:22:40 -03:00
# include <GCS_MAVLink/GCS.h>
2017-04-29 03:53:39 -03:00
# include <SRV_Channel/SRV_Channel.h>
2019-01-18 00:23:42 -04:00
# include <AP_Logger/AP_Logger.h>
2016-06-08 09:13:53 -03:00
# include <utility>
2016-11-30 00:49:19 -04:00
# include "AP_Airspeed.h"
2016-11-30 00:59:20 -04:00
# include "AP_Airspeed_MS4525.h"
2016-11-30 02:21:48 -04:00
# include "AP_Airspeed_MS5525.h"
2017-11-03 01:47:32 -03:00
# include "AP_Airspeed_SDP3X.h"
2018-07-11 23:05:12 -03:00
# include "AP_Airspeed_DLVR.h"
2016-11-30 00:49:19 -04:00
# include "AP_Airspeed_analog.h"
2017-11-03 01:47:32 -03:00
# include "AP_Airspeed_Backend.h"
2018-09-05 03:45:01 -03:00
# if HAL_WITH_UAVCAN
# include "AP_Airspeed_UAVCAN.h"
# endif
2012-07-15 22:21:20 -03:00
2016-01-29 07:38:33 -04:00
extern const AP_HAL : : HAL & hal ;
2015-11-03 09:46:29 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2016-11-30 00:49:19 -04:00
# define ARSPD_DEFAULT_TYPE TYPE_ANALOG
2013-11-28 06:43:43 -04:00
# define ARSPD_DEFAULT_PIN 1
2014-12-30 17:11:27 -04:00
# else
2016-11-30 00:49:19 -04:00
# define ARSPD_DEFAULT_TYPE TYPE_I2C_MS4525
# define ARSPD_DEFAULT_PIN 15
2014-03-31 14:55:23 -03:00
# endif
2016-10-31 06:48:22 -03:00
# if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
# define PSI_RANGE_DEFAULT 0.05
2013-06-02 22:40:06 -03:00
# endif
2016-06-08 21:10:54 -03:00
# ifndef PSI_RANGE_DEFAULT
# define PSI_RANGE_DEFAULT 1.0f
# endif
2012-07-15 22:21:20 -03:00
// table of user settable parameters
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_Airspeed : : var_info [ ] = {
2012-07-15 22:21:20 -03:00
2017-11-03 03:17:34 -03:00
// @Param: _TYPE
2016-11-30 00:49:19 -04:00
// @DisplayName: Airspeed type
// @Description: Type of airspeed sensor
2018-09-05 03:45:01 -03:00
// @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR,8:UAVCAN
2016-07-26 02:35:15 -03:00
// @User: Standard
2017-11-03 03:17:34 -03:00
AP_GROUPINFO_FLAGS ( " _TYPE " , 0 , AP_Airspeed , param [ 0 ] . type , ARSPD_DEFAULT_TYPE , AP_PARAM_FLAG_ENABLE ) ,
2012-07-15 22:21:20 -03:00
2017-11-03 03:17:34 -03:00
// @Param: _USE
2012-07-15 22:21:20 -03:00
// @DisplayName: Airspeed use
2017-04-29 03:53:39 -03:00
// @Description: use airspeed for flight control. When set to 0 airspeed sensor can be logged and displayed on a GCS but won't be used for flight. When set to 1 it will be logged and used. When set to 2 it will be only used when the throttle is zero, which can be useful in gliders with airspeed sensors behind a propeller
// @Values: 0:Don't Use,1:use,2:UseWhenZeroThrottle
2016-07-26 02:35:15 -03:00
// @User: Standard
2017-11-03 03:17:34 -03:00
AP_GROUPINFO ( " _USE " , 1 , AP_Airspeed , param [ 0 ] . use , 0 ) ,
2012-07-15 22:21:20 -03:00
2017-11-03 03:17:34 -03:00
// @Param: _OFFSET
2012-07-15 22:21:20 -03:00
// @DisplayName: Airspeed offset
// @Description: Airspeed calibration offset
// @Increment: 0.1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2017-11-03 03:17:34 -03:00
AP_GROUPINFO ( " _OFFSET " , 2 , AP_Airspeed , param [ 0 ] . offset , 0 ) ,
2012-07-15 22:21:20 -03:00
2017-11-03 03:17:34 -03:00
// @Param: _RATIO
2012-07-15 22:21:20 -03:00
// @DisplayName: Airspeed ratio
// @Description: Airspeed calibration ratio
// @Increment: 0.1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2017-11-03 03:17:34 -03:00
AP_GROUPINFO ( " _RATIO " , 3 , AP_Airspeed , param [ 0 ] . ratio , 1.9936f ) ,
2012-07-15 22:21:20 -03:00
2017-11-03 03:17:34 -03:00
// @Param: _PIN
2013-06-02 22:40:06 -03:00
// @DisplayName: Airspeed pin
2016-11-30 00:49:19 -04:00
// @Description: The pin number that the airspeed sensor is connected to for analog sensors. Set to 15 on the Pixhawk for the analog airspeed port.
2013-06-02 22:40:06 -03:00
// @User: Advanced
2017-11-03 03:17:34 -03:00
AP_GROUPINFO ( " _PIN " , 4 , AP_Airspeed , param [ 0 ] . pin , ARSPD_DEFAULT_PIN ) ,
2013-06-02 22:40:06 -03:00
2017-11-03 03:17:34 -03:00
// @Param: _AUTOCAL
2013-07-19 19:14:58 -03:00
// @DisplayName: Automatic airspeed ratio calibration
2018-07-23 20:47:40 -03:00
// @Description: If this is enabled then the autopilot will automatically adjust the ARSPD_RATIO during flight, based upon an estimation filter using ground speed and true airspeed. The automatic calibration will save the new ratio to EEPROM every 2 minutes if it changes by more than 5%. This option should be enabled for a calibration flight then disabled again when calibration is complete. Leaving it enabled all the time is not recommended.
2013-07-19 19:14:58 -03:00
// @User: Advanced
2017-11-03 03:17:34 -03:00
AP_GROUPINFO ( " _AUTOCAL " , 5 , AP_Airspeed , param [ 0 ] . autocal , 0 ) ,
2013-07-19 19:14:58 -03:00
2017-11-03 03:17:34 -03:00
// @Param: _TUBE_ORDER
2014-02-14 07:07:12 -04:00
// @DisplayName: Control pitot tube order
// @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the top connector on the sensor needs to be the dynamic pressure. If set to 1 then the bottom connector needs to be the dynamic pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft it receiving excessive pressure on the static port, which would otherwise be seen as a positive airspeed.
// @User: Advanced
2017-11-03 03:17:34 -03:00
AP_GROUPINFO ( " _TUBE_ORDER " , 6 , AP_Airspeed , param [ 0 ] . tube_order , 2 ) ,
2014-02-14 07:07:12 -04:00
2017-11-03 03:17:34 -03:00
// @Param: _SKIP_CAL
2014-11-13 06:12:37 -04:00
// @DisplayName: Skip airspeed calibration on startup
// @Description: This parameter allows you to skip airspeed offset calibration on startup, instead using the offset from the last calibration. This may be desirable if the offset variance between flights for your sensor is low and you want to avoid having to cover the pitot tube on each boot.
// @Values: 0:Disable,1:Enable
// @User: Advanced
2017-11-03 03:17:34 -03:00
AP_GROUPINFO ( " _SKIP_CAL " , 7 , AP_Airspeed , param [ 0 ] . skip_cal , 0 ) ,
2014-11-13 06:12:37 -04:00
2017-11-03 03:17:34 -03:00
// @Param: _PSI_RANGE
2016-06-08 21:10:54 -03:00
// @DisplayName: The PSI range of the device
// @Description: This parameter allows you to to set the PSI (pounds per square inch) range for your sensor. You should not change this unless you examine the datasheet for your device
// @User: Advanced
2017-11-03 03:17:34 -03:00
AP_GROUPINFO ( " _PSI_RANGE " , 8 , AP_Airspeed , param [ 0 ] . psi_range , PSI_RANGE_DEFAULT ) ,
2016-11-30 02:21:48 -04:00
2017-11-03 03:17:34 -03:00
// @Param: _BUS
2016-11-30 02:21:48 -04:00
// @DisplayName: Airspeed I2C bus
// @Description: The bus number of the I2C bus to look for the sensor on
2017-11-03 01:47:32 -03:00
// @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary)
2016-11-30 02:21:48 -04:00
// @User: Advanced
2017-11-03 03:17:34 -03:00
AP_GROUPINFO ( " _BUS " , 9 , AP_Airspeed , param [ 0 ] . bus , 1 ) ,
// @Param: _PRIMARY
// @DisplayName: Primary airspeed sensor
// @Description: This selects which airspeed sensor will be the primary if multiple sensors are found
// @Values: 0:FirstSensor,1:2ndSensor
// @User: Advanced
AP_GROUPINFO ( " _PRIMARY " , 10 , AP_Airspeed , primary_sensor , 0 ) ,
// @Param: 2_TYPE
// @DisplayName: Second Airspeed type
// @Description: Type of 2nd airspeed sensor
2018-09-05 03:45:01 -03:00
// @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR,8:UAVCAN
2017-11-03 03:17:34 -03:00
// @User: Standard
AP_GROUPINFO_FLAGS ( " 2_TYPE " , 11 , AP_Airspeed , param [ 1 ] . type , 0 , AP_PARAM_FLAG_ENABLE ) ,
// @Param: 2_USE
// @DisplayName: Enable use of 2nd airspeed sensor
// @Description: use airspeed for flight control. When set to 0 airspeed sensor can be logged and displayed on a GCS but won't be used for flight. When set to 1 it will be logged and used. When set to 2 it will be only used when the throttle is zero, which can be useful in gliders with airspeed sensors behind a propeller
// @Values: 0:Don't Use,1:use,2:UseWhenZeroThrottle
// @User: Standard
AP_GROUPINFO ( " 2_USE " , 12 , AP_Airspeed , param [ 1 ] . use , 0 ) ,
// @Param: 2_OFFSET
// @DisplayName: Airspeed offset for 2nd airspeed sensor
// @Description: Airspeed calibration offset
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO ( " 2_OFFSET " , 13 , AP_Airspeed , param [ 1 ] . offset , 0 ) ,
// @Param: 2_RATIO
// @DisplayName: Airspeed ratio for 2nd airspeed sensor
// @Description: Airspeed calibration ratio
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO ( " 2_RATIO " , 14 , AP_Airspeed , param [ 1 ] . ratio , 2 ) ,
// @Param: 2_PIN
// @DisplayName: Airspeed pin for 2nd airspeed sensor
// @Description: The pin number that the airspeed sensor is connected to for analog sensors. Set to 15 on the Pixhawk for the analog airspeed port.
// @User: Advanced
AP_GROUPINFO ( " 2_PIN " , 15 , AP_Airspeed , param [ 1 ] . pin , 0 ) ,
// @Param: 2_AUTOCAL
// @DisplayName: Automatic airspeed ratio calibration for 2nd airspeed sensor
2018-07-23 20:47:40 -03:00
// @Description: If this is enabled then the autopilot will automatically adjust the ARSPD_RATIO during flight, based upon an estimation filter using ground speed and true airspeed. The automatic calibration will save the new ratio to EEPROM every 2 minutes if it changes by more than 5%. This option should be enabled for a calibration flight then disabled again when calibration is complete. Leaving it enabled all the time is not recommended.
2017-11-03 03:17:34 -03:00
// @User: Advanced
AP_GROUPINFO ( " 2_AUTOCAL " , 16 , AP_Airspeed , param [ 1 ] . autocal , 0 ) ,
// @Param: 2_TUBE_ORDR
// @DisplayName: Control pitot tube order of 2nd airspeed sensor
// @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the top connector on the sensor needs to be the dynamic pressure. If set to 1 then the bottom connector needs to be the dynamic pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft it receiving excessive pressure on the static port, which would otherwise be seen as a positive airspeed.
// @User: Advanced
AP_GROUPINFO ( " 2_TUBE_ORDR " , 17 , AP_Airspeed , param [ 1 ] . tube_order , 2 ) ,
// @Param: 2_SKIP_CAL
// @DisplayName: Skip airspeed calibration on startup for 2nd sensor
// @Description: This parameter allows you to skip airspeed offset calibration on startup, instead using the offset from the last calibration. This may be desirable if the offset variance between flights for your sensor is low and you want to avoid having to cover the pitot tube on each boot.
// @Values: 0:Disable,1:Enable
// @User: Advanced
AP_GROUPINFO ( " 2_SKIP_CAL " , 18 , AP_Airspeed , param [ 1 ] . skip_cal , 0 ) ,
// @Param: 2_PSI_RANGE
// @DisplayName: The PSI range of the device for 2nd sensor
// @Description: This parameter allows you to to set the PSI (pounds per square inch) range for your sensor. You should not change this unless you examine the datasheet for your device
// @User: Advanced
AP_GROUPINFO ( " 2_PSI_RANGE " , 19 , AP_Airspeed , param [ 1 ] . psi_range , PSI_RANGE_DEFAULT ) ,
// @Param: 2_BUS
// @DisplayName: Airspeed I2C bus for 2nd sensor
// @Description: The bus number of the I2C bus to look for the sensor on
// @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary)
// @User: Advanced
AP_GROUPINFO ( " 2_BUS " , 20 , AP_Airspeed , param [ 1 ] . bus , 1 ) ,
2016-08-07 21:48:54 -03:00
2012-07-15 22:21:20 -03:00
AP_GROUPEND
} ;
2018-11-16 12:16:23 -04:00
/*
this scaling factor converts from the old system where we used a
0 to 4095 raw ADC value for 0 - 5 V to the new system which gets the
voltage in volts directly from the ADC driver
*/
# define SCALING_OLD_CALIBRATION 819 // 4095/5
2018-03-09 17:12:05 -04:00
2016-08-08 00:29:50 -03:00
AP_Airspeed : : AP_Airspeed ( )
2016-06-08 09:13:53 -03:00
{
2017-11-03 03:17:34 -03:00
for ( uint8_t i = 0 ; i < AIRSPEED_MAX_SENSORS ; i + + ) {
state [ i ] . EAS2TAS = 1 ;
}
2016-06-08 09:13:53 -03:00
AP_Param : : setup_object_defaults ( this , var_info ) ;
2018-03-04 20:13:48 -04:00
if ( _singleton ! = nullptr ) {
AP_HAL : : panic ( " AP_Airspeed must be singleton " ) ;
}
_singleton = this ;
2016-08-07 21:48:54 -03:00
}
2016-06-08 09:13:53 -03:00
2013-06-02 22:40:06 -03:00
void AP_Airspeed : : init ( )
{
2016-11-30 00:49:19 -04:00
// cope with upgrade from old system
2017-11-03 03:17:34 -03:00
if ( param [ 0 ] . pin . load ( ) & & param [ 0 ] . pin . get ( ) ! = 65 ) {
param [ 0 ] . type . set_default ( TYPE_ANALOG ) ;
2016-11-30 00:49:19 -04:00
}
2017-11-03 03:17:34 -03:00
for ( uint8_t i = 0 ; i < AIRSPEED_MAX_SENSORS ; i + + ) {
state [ i ] . calibration . init ( param [ i ] . ratio ) ;
state [ i ] . last_saved_ratio = param [ i ] . ratio ;
switch ( ( enum airspeed_type ) param [ i ] . type . get ( ) ) {
case TYPE_NONE :
// nothing to do
break ;
case TYPE_I2C_MS4525 :
sensor [ i ] = new AP_Airspeed_MS4525 ( * this , i ) ;
break ;
case TYPE_ANALOG :
sensor [ i ] = new AP_Airspeed_Analog ( * this , i ) ;
break ;
case TYPE_I2C_MS5525 :
sensor [ i ] = new AP_Airspeed_MS5525 ( * this , i , AP_Airspeed_MS5525 : : MS5525_ADDR_AUTO ) ;
break ;
case TYPE_I2C_MS5525_ADDRESS_1 :
sensor [ i ] = new AP_Airspeed_MS5525 ( * this , i , AP_Airspeed_MS5525 : : MS5525_ADDR_1 ) ;
break ;
case TYPE_I2C_MS5525_ADDRESS_2 :
sensor [ i ] = new AP_Airspeed_MS5525 ( * this , i , AP_Airspeed_MS5525 : : MS5525_ADDR_2 ) ;
break ;
case TYPE_I2C_SDP3X :
sensor [ i ] = new AP_Airspeed_SDP3X ( * this , i ) ;
break ;
2018-07-11 23:05:12 -03:00
case TYPE_I2C_DLVR :
# if !HAL_MINIMIZE_FEATURES
sensor [ i ] = new AP_Airspeed_DLVR ( * this , i ) ;
# endif // !HAL_MINIMIZE_FEATURES
break ;
2018-09-05 03:45:01 -03:00
case TYPE_UAVCAN :
# if HAL_WITH_UAVCAN
sensor [ i ] = AP_Airspeed_UAVCAN : : probe ( * this , i ) ;
# endif
break ;
2017-11-03 03:17:34 -03:00
}
2018-09-05 03:45:01 -03:00
2017-11-03 03:17:34 -03:00
if ( sensor [ i ] & & ! sensor [ i ] - > init ( ) ) {
2018-09-20 19:47:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Airspeed %u init failed " , i + 1 ) ;
2017-11-03 03:17:34 -03:00
delete sensor [ i ] ;
sensor [ i ] = nullptr ;
}
2016-11-30 00:49:19 -04:00
}
2013-06-02 22:40:06 -03:00
}
// read the airspeed sensor
2017-11-03 03:17:34 -03:00
float AP_Airspeed : : get_pressure ( uint8_t i )
2013-06-02 22:40:06 -03:00
{
2017-11-03 03:17:34 -03:00
if ( ! enabled ( i ) ) {
2013-06-02 22:40:06 -03:00
return 0 ;
}
2017-11-03 03:17:34 -03:00
if ( state [ i ] . hil_set ) {
state [ i ] . healthy = true ;
return state [ i ] . hil_pressure ;
2014-08-10 05:16:53 -03:00
}
2013-09-28 05:40:29 -03:00
float pressure = 0 ;
2017-11-03 03:17:34 -03:00
if ( sensor [ i ] ) {
state [ i ] . healthy = sensor [ i ] - > get_differential_pressure ( pressure ) ;
2013-06-02 22:40:06 -03:00
}
2013-09-28 05:40:29 -03:00
return pressure ;
2013-06-02 22:40:06 -03:00
}
2014-01-27 19:35:35 -04:00
// get a temperature reading if possible
2017-11-03 03:17:34 -03:00
bool AP_Airspeed : : get_temperature ( uint8_t i , float & temperature )
2014-01-27 19:35:35 -04:00
{
2017-11-03 03:17:34 -03:00
if ( ! enabled ( i ) ) {
2014-01-27 19:35:35 -04:00
return false ;
}
2017-11-03 03:17:34 -03:00
if ( sensor [ i ] ) {
return sensor [ i ] - > get_temperature ( temperature ) ;
2014-01-27 19:35:35 -04:00
}
return false ;
}
2017-11-03 03:17:34 -03:00
// calibrate the zero offset for the airspeed. This must be called at
// least once before the get_airspeed() interface can be used
2014-11-13 06:12:37 -04:00
void AP_Airspeed : : calibrate ( bool in_startup )
2012-07-15 22:21:20 -03:00
{
2017-11-03 03:17:34 -03:00
for ( uint8_t i = 0 ; i < AIRSPEED_MAX_SENSORS ; i + + ) {
if ( ! enabled ( i ) ) {
continue ;
}
if ( state [ i ] . use_zero_offset ) {
param [ i ] . offset . set ( 0 ) ;
continue ;
}
if ( in_startup & & param [ i ] . skip_cal ) {
continue ;
}
state [ i ] . cal . start_ms = AP_HAL : : millis ( ) ;
state [ i ] . cal . count = 0 ;
state [ i ] . cal . sum = 0 ;
state [ i ] . cal . read_count = 0 ;
2014-11-13 06:12:37 -04:00
}
2018-03-27 23:30:50 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Airspeed calibration started " ) ;
2016-05-24 01:22:40 -03:00
}
2016-06-26 21:52:59 -03:00
/*
2017-11-03 03:17:34 -03:00
update async airspeed zero offset calibration
2016-06-26 21:52:59 -03:00
*/
2017-11-03 03:17:34 -03:00
void AP_Airspeed : : update_calibration ( uint8_t i , float raw_pressure )
2016-05-24 01:22:40 -03:00
{
2017-11-03 03:17:34 -03:00
if ( ! enabled ( i ) | | state [ i ] . cal . start_ms = = 0 ) {
return ;
}
2016-12-05 21:19:57 -04:00
// consider calibration complete when we have at least 15 samples
2016-06-26 21:52:59 -03:00
// over at least 1 second
2017-11-03 03:17:34 -03:00
if ( AP_HAL : : millis ( ) - state [ i ] . cal . start_ms > = 1000 & &
state [ i ] . cal . read_count > 15 ) {
if ( state [ i ] . cal . count = = 0 ) {
2018-09-20 19:47:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Airspeed %u unhealthy " , i + 1 ) ;
2016-05-24 01:22:40 -03:00
} else {
2018-09-20 19:47:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Airspeed %u calibrated " , i + 1 ) ;
2017-11-03 03:17:34 -03:00
param [ i ] . offset . set_and_save ( state [ i ] . cal . sum / state [ i ] . cal . count ) ;
2013-09-28 05:40:29 -03:00
}
2017-11-03 03:17:34 -03:00
state [ i ] . cal . start_ms = 0 ;
2013-09-28 05:40:29 -03:00
return ;
2012-08-17 03:08:43 -03:00
}
2016-11-30 02:21:48 -04:00
// we discard the first 5 samples
2017-11-03 03:17:34 -03:00
if ( state [ i ] . healthy & & state [ i ] . cal . read_count > 5 ) {
state [ i ] . cal . sum + = raw_pressure ;
state [ i ] . cal . count + + ;
2016-05-24 01:22:40 -03:00
}
2017-11-03 03:17:34 -03:00
state [ i ] . cal . read_count + + ;
2012-07-15 22:21:20 -03:00
}
2017-11-03 03:17:34 -03:00
// read one airspeed sensor
void AP_Airspeed : : read ( uint8_t i )
2012-07-15 22:21:20 -03:00
{
2012-08-17 03:08:43 -03:00
float airspeed_pressure ;
2017-11-03 03:17:34 -03:00
if ( ! enabled ( i ) | | ! sensor [ i ] ) {
2012-08-17 03:08:43 -03:00
return ;
}
2018-06-10 19:45:11 -03:00
bool prev_healthy = state [ i ] . healthy ;
2017-11-03 03:17:34 -03:00
float raw_pressure = get_pressure ( i ) ;
if ( state [ i ] . cal . start_ms ! = 0 ) {
update_calibration ( i , raw_pressure ) ;
2016-05-24 01:22:40 -03:00
}
2017-11-03 03:17:34 -03:00
airspeed_pressure = raw_pressure - param [ i ] . offset ;
2014-02-14 07:07:12 -04:00
2014-11-13 02:49:04 -04:00
// remember raw pressure for logging
2017-11-03 03:17:34 -03:00
state [ i ] . corrected_pressure = airspeed_pressure ;
2014-11-13 02:49:04 -04:00
2017-09-19 16:28:56 -03:00
// filter before clamping positive
2018-06-10 19:45:11 -03:00
if ( ! prev_healthy ) {
// if the previous state was not healthy then we should not
// use an IIR filter, otherwise a bad reading will last for
// some time after the sensor becomees healthy again
state [ i ] . filtered_pressure = airspeed_pressure ;
} else {
state [ i ] . filtered_pressure = 0.7f * state [ i ] . filtered_pressure + 0.3f * airspeed_pressure ;
}
2017-09-19 16:28:56 -03:00
2014-02-13 02:21:06 -04:00
/*
2017-09-19 16:28:56 -03:00
we support different pitot tube setups so user can choose if
2014-02-14 07:07:12 -04:00
they want to be able to detect pressure on the static port
2014-02-13 02:21:06 -04:00
*/
2017-11-03 03:17:34 -03:00
switch ( ( enum pitot_tube_order ) param [ i ] . tube_order . get ( ) ) {
2014-02-14 07:07:12 -04:00
case PITOT_TUBE_ORDER_NEGATIVE :
2017-11-03 03:17:34 -03:00
state [ i ] . last_pressure = - airspeed_pressure ;
state [ i ] . raw_airspeed = sqrtf ( MAX ( - airspeed_pressure , 0 ) * param [ i ] . ratio ) ;
state [ i ] . airspeed = sqrtf ( MAX ( - state [ i ] . filtered_pressure , 0 ) * param [ i ] . ratio ) ;
2017-09-19 16:28:56 -03:00
break ;
2014-02-14 07:07:12 -04:00
case PITOT_TUBE_ORDER_POSITIVE :
2017-11-03 03:17:34 -03:00
state [ i ] . last_pressure = airspeed_pressure ;
state [ i ] . raw_airspeed = sqrtf ( MAX ( airspeed_pressure , 0 ) * param [ i ] . ratio ) ;
state [ i ] . airspeed = sqrtf ( MAX ( state [ i ] . filtered_pressure , 0 ) * param [ i ] . ratio ) ;
2014-02-14 07:07:12 -04:00
break ;
case PITOT_TUBE_ORDER_AUTO :
default :
2017-11-03 03:17:34 -03:00
state [ i ] . last_pressure = fabsf ( airspeed_pressure ) ;
state [ i ] . raw_airspeed = sqrtf ( fabsf ( airspeed_pressure ) * param [ i ] . ratio ) ;
state [ i ] . airspeed = sqrtf ( fabsf ( state [ i ] . filtered_pressure ) * param [ i ] . ratio ) ;
2014-02-14 07:07:12 -04:00
break ;
2014-02-13 02:21:06 -04:00
}
2017-09-19 16:28:56 -03:00
2018-09-20 19:47:39 -03:00
if ( state [ i ] . last_pressure < - 32 ) {
// we're reading more than about -8m/s. The user probably has
// the ports the wrong way around
state [ i ] . healthy = false ;
}
2017-11-03 03:17:34 -03:00
state [ i ] . last_update_ms = AP_HAL : : millis ( ) ;
}
// read all airspeed sensors
2018-11-16 12:16:23 -04:00
void AP_Airspeed : : update ( bool log )
2017-11-03 03:17:34 -03:00
{
for ( uint8_t i = 0 ; i < AIRSPEED_MAX_SENSORS ; i + + ) {
read ( i ) ;
}
# if 1
// debugging until we get MAVLink support for 2nd airspeed sensor
if ( enabled ( 1 ) ) {
2018-05-13 06:29:47 -03:00
gcs ( ) . send_named_float ( " AS2 " , get_airspeed ( 1 ) ) ;
2017-11-03 03:17:34 -03:00
}
# endif
2018-11-16 12:16:23 -04:00
if ( log ) {
2019-01-18 00:23:42 -04:00
AP_Logger * _dataflash = AP_Logger : : instance ( ) ;
2018-11-16 12:16:23 -04:00
if ( _dataflash ! = nullptr ) {
2019-01-18 00:24:08 -04:00
_dataflash - > Write_Airspeed ( * this ) ;
2018-11-16 12:16:23 -04:00
}
}
2018-03-09 17:12:05 -04:00
// setup primary
if ( healthy ( primary_sensor . get ( ) ) ) {
primary = primary_sensor . get ( ) ;
2018-03-09 04:07:34 -04:00
return ;
}
2018-03-09 17:12:05 -04:00
for ( uint8_t i = 0 ; i < AIRSPEED_MAX_SENSORS ; i + + ) {
if ( healthy ( i ) ) {
primary = i ;
break ;
2017-11-03 03:17:34 -03:00
}
}
2012-07-15 22:21:20 -03:00
}
2014-02-17 18:24:14 -04:00
void AP_Airspeed : : setHIL ( float airspeed , float diff_pressure , float temperature )
{
2017-11-03 03:17:34 -03:00
state [ 0 ] . raw_airspeed = airspeed ;
state [ 0 ] . airspeed = airspeed ;
state [ 0 ] . last_pressure = diff_pressure ;
state [ 0 ] . last_update_ms = AP_HAL : : millis ( ) ;
state [ 0 ] . hil_pressure = diff_pressure ;
state [ 0 ] . hil_set = true ;
state [ 0 ] . healthy = true ;
2014-02-17 18:24:14 -04:00
}
2017-04-29 03:53:39 -03:00
2017-11-03 03:17:34 -03:00
bool AP_Airspeed : : use ( uint8_t i ) const
2017-04-29 03:53:39 -03:00
{
2017-11-03 03:17:34 -03:00
if ( ! enabled ( i ) | | ! param [ i ] . use ) {
2017-04-29 03:53:39 -03:00
return false ;
}
2017-11-03 03:17:34 -03:00
if ( param [ i ] . use = = 2 & & SRV_Channels : : get_output_scaled ( SRV_Channel : : k_throttle ) ! = 0 ) {
2017-04-29 03:53:39 -03:00
// special case for gliders with airspeed sensors behind the
// propeller. Allow airspeed to be disabled when throttle is
// running
return false ;
}
return true ;
}
2017-11-03 03:17:34 -03:00
/*
return true if all enabled sensors are healthy
*/
bool AP_Airspeed : : all_healthy ( void ) const
{
for ( uint8_t i = 0 ; i < AIRSPEED_MAX_SENSORS ; i + + ) {
if ( enabled ( i ) & & ! healthy ( i ) ) {
return false ;
}
}
return true ;
}
2018-03-04 20:13:48 -04:00
// singleton instance
AP_Airspeed * AP_Airspeed : : _singleton ;