CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_Airspeed/AP_Airspeed_Params.cpp
Views: 1798
1
2
/*
3
This program is free software: you can redistribute it and/or modify
4
it under the terms of the GNU General Public License as published by
5
the Free Software Foundation, either version 3 of the License, or
6
(at your option) any later version.
7
8
This program is distributed in the hope that it will be useful,
9
but WITHOUT ANY WARRANTY; without even the implied warranty of
10
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11
GNU General Public License for more details.
12
13
You should have received a copy of the GNU General Public License
14
along with this program. If not, see <http://www.gnu.org/licenses/>.
15
*/
16
17
#include "AP_Airspeed_config.h"
18
19
#if AP_AIRSPEED_ENABLED
20
21
#include "AP_Airspeed.h"
22
23
#include <AP_Vehicle/AP_Vehicle_Type.h>
24
25
// Dummy the AP_Airspeed class to allow building Airspeed only for plane, rover, sub, and copter & heli 2MB boards
26
// This could be removed once the build system allows for APM_BUILD_TYPE in header files
27
// note that this is re-definition of the one in AP_Airspeed.cpp, can't be shared as vehicle dependences cant go in header files
28
#ifndef AP_AIRSPEED_DUMMY_METHODS_ENABLED
29
#define AP_AIRSPEED_DUMMY_METHODS_ENABLED ((APM_BUILD_COPTER_OR_HELI && BOARD_FLASH_SIZE <= 1024) || \
30
APM_BUILD_TYPE(APM_BUILD_AntennaTracker) || APM_BUILD_TYPE(APM_BUILD_Blimp))
31
#endif
32
33
#if !AP_AIRSPEED_DUMMY_METHODS_ENABLED
34
35
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
36
#define PSI_RANGE_DEFAULT 0.05
37
#endif
38
39
#ifndef PSI_RANGE_DEFAULT
40
#define PSI_RANGE_DEFAULT 1.0f
41
#endif
42
43
// table of user settable parameters
44
const AP_Param::GroupInfo AP_Airspeed_Params::var_info[] = {
45
46
// @Param: TYPE
47
// @DisplayName: Airspeed type
48
// @Description: Type of airspeed sensor
49
// @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-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,16:ExternalAHRS,100:SITL
50
// @User: Standard
51
AP_GROUPINFO_FLAGS("TYPE", 1, AP_Airspeed_Params, type, 0, AP_PARAM_FLAG_ENABLE),
52
53
#ifndef HAL_BUILD_AP_PERIPH
54
// @Param: USE
55
// @DisplayName: Airspeed use
56
// @Description: Enables airspeed use for automatic throttle modes and replaces control from THR_TRIM. Continues to display and log airspeed if set to 0. Uses airspeed for control if set to 1. Only uses airspeed when throttle = 0 if set to 2 (useful for gliders with airspeed sensors behind propellers).
57
// @Description{Copter, Blimp, Rover, Sub}: This parameter is not used by this vehicle. Always set to 0.
58
// @Values: 0:DoNotUse,1:Use,2:UseWhenZeroThrottle
59
// @User: Standard
60
AP_GROUPINFO("USE", 2, AP_Airspeed_Params, use, 0),
61
62
// @Param: OFFSET
63
// @DisplayName: Airspeed offset
64
// @Description: Airspeed calibration offset
65
// @Increment: 0.1
66
// @User: Advanced
67
AP_GROUPINFO("OFFSET", 3, AP_Airspeed_Params, offset, 0),
68
69
// @Param: RATIO
70
// @DisplayName: Airspeed ratio
71
// @Description: Calibrates pitot tube pressure to velocity. Increasing this value will indicate a higher airspeed at any given dynamic pressure.
72
// @Increment: 0.1
73
// @User: Advanced
74
AP_GROUPINFO("RATIO", 4, AP_Airspeed_Params, ratio, 2),
75
76
// @Param: PIN
77
// @DisplayName: Airspeed pin
78
// @Description: The pin number that the airspeed sensor is connected to for analog sensors. Values for some autopilots are given as examples. Search wiki for "Analog pins".
79
// @Values: -1:Disabled, 2:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 5:Navigator, 13:Pixhawk2_PM2/CubeOrange_PM2, 14:CubeOrange, 16:Durandal, 100:PX4-v1
80
// @User: Advanced
81
AP_GROUPINFO("PIN", 5, AP_Airspeed_Params, pin, 0),
82
#endif // HAL_BUILD_AP_PERIPH
83
84
#if AP_AIRSPEED_AUTOCAL_ENABLE
85
// @Param: AUTOCAL
86
// @DisplayName: Automatic airspeed ratio calibration
87
// @DisplayName{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
88
// @Description: Enables automatic adjustment of airspeed ratio during a calibration flight based on estimation of ground speed and true airspeed. New ratio saved every 2 minutes if change is > 5%. Should not be left enabled.
89
// @User: Advanced
90
AP_GROUPINFO("AUTOCAL", 6, AP_Airspeed_Params, autocal, 0),
91
#endif
92
93
#ifndef HAL_BUILD_AP_PERIPH
94
// @Param: TUBE_ORDR
95
// @DisplayName: Control pitot tube order
96
// @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 first (often the top) connector on the sensor needs to be the stagnation pressure (the pressure at the tip of the pitot tube). If set to 1 then the second (often the bottom) connector needs to be the stagnation 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 is receiving excessive pressure on the static port compared to the stagnation port such as during a stall, which would otherwise be seen as a positive airspeed.
97
// @User: Advanced
98
// @Values: 0:Normal,1:Swapped,2:Auto Detect
99
AP_GROUPINFO("TUBE_ORDR", 7, AP_Airspeed_Params, tube_order, 2),
100
101
// @Param: SKIP_CAL
102
// @DisplayName: Skip airspeed offset calibration on startup
103
// @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.
104
// @Values: 0:Disable,1:Enable
105
// @User: Advanced
106
AP_GROUPINFO("SKIP_CAL", 8, AP_Airspeed_Params, skip_cal, 0),
107
#endif // HAL_BUILD_AP_PERIPH
108
109
// @Param: PSI_RANGE
110
// @DisplayName: The PSI range of the device
111
// @Description: This parameter allows you 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
112
// @User: Advanced
113
AP_GROUPINFO("PSI_RANGE", 9, AP_Airspeed_Params, psi_range, PSI_RANGE_DEFAULT),
114
115
#ifndef HAL_BUILD_AP_PERIPH
116
// @Param: BUS
117
// @DisplayName: Airspeed I2C bus
118
// @Description: Bus number of the I2C bus where the airspeed sensor is connected. May not correspond to board's I2C bus number labels. Retry another bus and reboot if airspeed sensor fails to initialize.
119
// @Values: 0:Bus0,1:Bus1,2:Bus2
120
// @RebootRequired: True
121
// @User: Advanced
122
AP_GROUPINFO("BUS", 10, AP_Airspeed_Params, bus, 1),
123
#endif // HAL_BUILD_AP_PERIPH
124
125
// @Param: DEVID
126
// @DisplayName: Airspeed ID
127
// @Description: Airspeed sensor ID, taking into account its type, bus and instance
128
// @ReadOnly: True
129
// @User: Advanced
130
AP_GROUPINFO_FLAGS("DEVID", 11, AP_Airspeed_Params, bus_id, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
131
132
AP_GROUPEND
133
};
134
135
AP_Airspeed_Params::AP_Airspeed_Params(void)
136
{
137
AP_Param::setup_object_defaults(this, var_info);
138
}
139
140
#else // dummy implementation:
141
142
AP_Airspeed_Params::AP_Airspeed_Params(void) {};
143
const AP_Param::GroupInfo AP_Airspeed_Params::var_info[] = { AP_GROUPEND };
144
145
#endif
146
147
#endif // AP_AIRSPEED_ENABLED
148
149