Line data Source code
1 : /*
2 : * This file is free software: you can redistribute it and/or modify it
3 : * under the terms of the GNU General Public License as published by the
4 : * Free Software Foundation, either version 3 of the License, or
5 : * (at your option) any later version.
6 : *
7 : * This file is distributed in the hope that it will be useful, but
8 : * WITHOUT ANY WARRANTY; without even the implied warranty of
9 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10 : * See the GNU General Public License for more details.
11 : *
12 : * You should have received a copy of the GNU General Public License along
13 : * with this program. If not, see <http://www.gnu.org/licenses/>.
14 : *
15 : * Code by Charlie Johnson
16 : */
17 :
18 : #include "AP_BattMonitor_config.h"
19 :
20 : #if AP_BATTERY_FUELLEVEL_ANALOG_ENABLED
21 :
22 : #include <AP_HAL/AP_HAL.h>
23 : #include <AP_Math/AP_Math.h>
24 :
25 : #include "AP_BattMonitor_FuelLevel_Analog.h"
26 :
27 : extern const AP_HAL::HAL& hal;
28 :
29 : const AP_Param::GroupInfo AP_BattMonitor_FuelLevel_Analog::var_info[] = {
30 :
31 : // @Param: FL_VLT_MIN
32 : // @DisplayName: Empty fuel level voltage
33 : // @Description: The voltage seen on the analog pin when the fuel tank is empty. Note: For this type of battery monitor, the voltage seen by the analog pin is displayed as battery voltage on a GCS.
34 : // @Range: 0.01 10
35 : // @Units: V
36 : // @User: Advanced
37 : AP_GROUPINFO("FL_VLT_MIN", 40, AP_BattMonitor_FuelLevel_Analog, _fuel_level_empty_voltage, 0.5),
38 :
39 : // @Param: FL_V_MULT
40 : // @DisplayName: Fuel level voltage multiplier
41 : // @Description: Voltage multiplier to determine what the full tank voltage reading is. This is calculated as 1 / (Voltage_Full - Voltage_Empty) Note: For this type of battery monitor, the voltage seen by the analog pin is displayed as battery voltage on a GCS.
42 : // @Range: 0.01 10
43 : // @User: Advanced
44 : AP_GROUPINFO("FL_V_MULT", 41, AP_BattMonitor_FuelLevel_Analog, _fuel_level_voltage_mult, 0.5),
45 :
46 : // @Param: FL_FLTR
47 : // @DisplayName: Fuel level filter frequency
48 : // @Description: Filter frequency in Hertz where a low pass filter is used. This is used to filter out tank slosh from the fuel level reading. A value of -1 disables the filter and unfiltered voltage is used to determine the fuel level. The suggested values at in the range of 0.2 Hz to 0.5 Hz.
49 : // @Range: -1 1
50 : // @User: Advanced
51 : // @Units: Hz
52 : // @RebootRequired: True
53 : AP_GROUPINFO("FL_FLTR", 42, AP_BattMonitor_FuelLevel_Analog, _fuel_level_filter_frequency, 0.3),
54 :
55 : // @Param: FL_PIN
56 : // @DisplayName: Fuel level analog pin number
57 : // @Description: Analog input pin that fuel level sensor is connected to.Analog Airspeed or RSSI ports can be used for Analog input( some autopilots provide others also). Values for some autopilots are given as examples. Search wiki for "Analog pins".
58 : // @Values: -1:Disabled, 2:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 5:Navigator, 13:Pixhawk2_PM2/CubeOrange_PM2, 14:CubeOrange, 16:Durandal, 100:PX4-v1
59 : // @Range: -1 127
60 : AP_GROUPINFO("FL_PIN", 43, AP_BattMonitor_FuelLevel_Analog, _pin, -1),
61 :
62 : // @Param: FL_FF
63 : // @DisplayName: First order term
64 : // @Description: First order polynomial fit term
65 : // @Range: -10 10
66 : // @User: Advanced
67 : AP_GROUPINFO("FL_FF", 45, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_first_order_coeff, 1),
68 :
69 : // @Param: FL_FS
70 : // @DisplayName: Second order term
71 : // @Description: Second order polynomial fit term
72 : // @Range: -10 10
73 : // @User: Advanced
74 : AP_GROUPINFO("FL_FS", 46, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_second_order_coeff, 0),
75 :
76 : // @Param: FL_FT
77 : // @DisplayName: Third order term
78 : // @Description: Third order polynomial fit term
79 : // @Range: -10 10
80 : // @User: Advanced
81 : AP_GROUPINFO("FL_FT", 47, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_third_order_coeff, 0),
82 :
83 : // @Param: FL_OFF
84 : // @DisplayName: Offset term
85 : // @Description: Offset polynomial fit term
86 : // @Range: -10 10
87 : // @User: Advanced
88 : AP_GROUPINFO("FL_OFF", 48, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_offset, 0),
89 :
90 : // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
91 :
92 : AP_GROUPEND
93 : };
94 :
95 : // Constructor
96 0 : AP_BattMonitor_FuelLevel_Analog::AP_BattMonitor_FuelLevel_Analog(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms) :
97 0 : AP_BattMonitor_Backend(mon, mon_state, params)
98 : {
99 0 : AP_Param::setup_object_defaults(this, var_info);
100 0 : _state.var_info = var_info;
101 :
102 0 : _analog_source = hal.analogin->channel(_pin);
103 :
104 : // create a low pass filter
105 : // use a pole at 0.3 Hz if the filter is not being used
106 0 : _voltage_filter.set_cutoff_frequency((_fuel_level_filter_frequency >= 0) ? _fuel_level_filter_frequency : 0.3f);
107 0 : }
108 :
109 : /*
110 : read - read the "voltage" and "current"
111 : */
112 0 : void AP_BattMonitor_FuelLevel_Analog::read()
113 : {
114 0 : if (_analog_source == nullptr) {
115 0 : return;
116 : }
117 :
118 0 : if (!_analog_source->set_pin(_pin)) {
119 0 : _state.healthy = false;
120 0 : return;
121 : }
122 0 : _state.healthy = true;
123 :
124 : // get a dt and a timestamp
125 0 : const uint32_t tnow = AP_HAL::micros();
126 0 : const uint32_t dt_us = tnow - _state.last_time_micros;
127 :
128 : // get voltage from an ADC pin
129 0 : const float raw_voltage = _analog_source->voltage_average();
130 :
131 : // Converting sensor reading to actual volume in tank in Litres (quadratic fit)
132 : const float voltage =
133 0 : (_fuel_fit_third_order_coeff * raw_voltage * raw_voltage * raw_voltage) +
134 0 : (_fuel_fit_second_order_coeff * raw_voltage * raw_voltage) +
135 0 : (_fuel_fit_first_order_coeff * raw_voltage) +
136 0 : _fuel_fit_offset;
137 :
138 0 : const float filtered_voltage = _voltage_filter.apply(voltage, float(dt_us) * 1.0e-6f);
139 :
140 : // output the ADC voltage to the voltage field for easier calibration of sensors
141 : // also output filtered voltage as a measure of tank slosh filtering
142 : // this could be useful for tuning the LPF
143 0 : const float &voltage_used = (_fuel_level_filter_frequency >= 0) ? filtered_voltage : voltage;
144 :
145 0 : _state.voltage = filtered_voltage;
146 :
147 : // this driver assumes that CAPACITY is set to tank volume in millilitres.
148 : // _fuel_level_voltage_mult is calculated by the user as 1 / (full_voltage - empty_voltage)
149 0 : const float fuel_level_ratio = (voltage_used - _fuel_level_empty_voltage) * _fuel_level_voltage_mult;
150 0 : const float fuel_level_used_ratio = 1.0 - fuel_level_ratio;
151 :
152 : // map consumed_mah to consumed millilitres
153 0 : _state.consumed_mah = fuel_level_used_ratio * _params._pack_capacity;
154 :
155 0 : _state.current_amps = 0;
156 :
157 : // map consumed_wh using fixed voltage of 1
158 0 : _state.consumed_wh = _state.consumed_mah;
159 :
160 : // record time
161 0 : _state.last_time_micros = tnow;
162 : }
163 :
164 : #endif // AP_BATTERY_FUELLEVEL_ANALOG_ENABLED
|
