Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: Merge in polynomial fit structure to FuelLevel_Analog

Browse Source 
Update parameters and make current false
This commit is contained in:
Jonathan Loong 2023-11-12 21:46:51 -08:00 committed by Andrew Tridgell
parent 044760cfb9
commit 3f99891404
2 changed files with 59 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp
View File

@ -58,6 +58,42 @@ const AP_Param::GroupInfo AP_BattMonitor_FuelLevel_Analog::var_info[] = {
// @Values: -1:Not Used,11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6/Pixhawk2 ADC,103:Pixhawk SBUS
AP_GROUPINFO("FL_PIN", 43, AP_BattMonitor_FuelLevel_Analog, _pin, -1),
// @Param: FL_VLT_MAX
// @DisplayName: Full fuel level voltage
// @Description: The voltage seen on the analog pin when the fuel tank is full.
// @Range: 0 10
// @Units: V
// @User: Advanced
AP_GROUPINFO("FL_VLT_MAX", 44, AP_BattMonitor_FuelLevel_Analog, _fuel_level_max_voltage, -1),
// @Param: FL_FF
// @DisplayName: First order term
// @Description: First order polynomial fit term
// @Range: 0 10
// @User: Advanced
AP_GROUPINFO("FL_FF", 45, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_first_order_coeff, 1),
// @Param: FL_FS
// @DisplayName: Second order term
// @Description: Second order polynomial fit term
// @Range: 0 10
// @User: Advanced
AP_GROUPINFO("FL_FS", 46, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_second_order_coeff, 0),
// @Param: FL_FT
// @DisplayName: Third order term
// @Description: Third order polynomial fit term
// @Range: 0 10
// @User: Advanced
AP_GROUPINFO("FL_FT", 47, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_third_order_coeff, 0),
// @Param: FL_OFF
// @DisplayName: Offset term
// @Description: Offset polynomial fit term
// @Range: 0 10
// @User: Advanced
AP_GROUPINFO("FL_OFF", 48, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_offset, 0),
// Param indexes must be between 40 and 49 to avoid conflict with other battery monitor param tables loaded by pointer
AP_GROUPEND
@ -96,16 +132,24 @@ void AP_BattMonitor_FuelLevel_Analog::read()
const uint32_t tnow = AP_HAL::micros();
const uint32_t dt_us = tnow - _state.last_time_micros;
// get voltage from an ADC pin and filter it
const float voltage = _analog_source->voltage_average();
// get voltage from an ADC pin
const float raw_voltage = _analog_source->voltage_average();
// Converting sensor reading to actual volume in tank in Litres (quadratic fit)
const float voltage =
(_fuel_fit_third_order_coeff * raw_voltage * raw_voltage * raw_voltage) +
(_fuel_fit_second_order_coeff * raw_voltage * raw_voltage) +
(_fuel_fit_first_order_coeff * raw_voltage) +
_fuel_fit_offset;
const float filtered_voltage = _voltage_filter.apply(voltage, float(dt_us) * 1.0e-6f);
const float &voltage_used = (_fuel_level_filter_frequency >= 0) ? filtered_voltage : voltage;
// output the ADC voltage to the voltage field for easier calibration of sensors
// also output filtered voltage as a measure of tank slosh filtering
// this could be useful for tuning the LPF
_state.voltage = voltage;
_state.current_amps = filtered_voltage;
const float &voltage_used = (_fuel_level_filter_frequency >= 0) ? filtered_voltage : voltage;
_state.voltage = filtered_voltage;
// this driver assumes that CAPACITY is set to tank volume in millilitres.
// _fuel_level_voltage_mult is calculated by the user as 1 / (full_voltage - empty_voltage)
@ -115,6 +159,8 @@ void AP_BattMonitor_FuelLevel_Analog::read()
// map consumed_mah to consumed millilitres
_state.consumed_mah = fuel_level_used_ratio * _params._pack_capacity;
_state.current_amps = 0;
// map consumed_wh using fixed voltage of 1
_state.consumed_wh = _state.consumed_mah;

10
libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.h
View File

@ -23,6 +23,7 @@
#include <Filter/LowPassFilter.h>
#include "AP_BattMonitor.h"
#include <Filter/Filter.h> // Filter library
class AP_BattMonitor_FuelLevel_Analog : public AP_BattMonitor_Backend
{
@ -40,20 +41,25 @@ public:
bool has_consumed_energy() const override { return true; }
// returns true if battery monitor provides current info
bool has_current() const override { return true; }
bool has_current() const override { return false; }
void init(void) override {}
private:
AP_Float _fuel_level_empty_voltage;
AP_Float _fuel_level_max_voltage;
AP_Float _fuel_level_voltage_mult;
AP_Float _fuel_level_filter_frequency;
AP_Int8 _pin;
AP_Float _fuel_fit_first_order_coeff;
AP_Float _fuel_fit_second_order_coeff;
AP_Float _fuel_fit_third_order_coeff;
AP_Float _fuel_fit_offset;
AP_HAL::AnalogSource *_analog_source;
LowPassFilterFloat _voltage_filter;
};
#endif // AP_BATTERY_FUELLEVEL_ANALOG_ENABLED