11#include " powermonitor.h"
22#include < ArduinoLog.h>
33#include " configuration.h"
4- #include < INA219.h>
4+ #include < array>
5+ #include < utility>
6+ #include " settings.h"
57
6- APB::PowerMonitor &APB::PowerMonitor::Instance = *new APB::PowerMonitor();
78
8- struct APB ::PowerMonitor::Private {
9- INA219 ina219{APB_INA1219_ADDRESS};
10- PowerMonitor::Status status;
11- Task loopTask;
12- };
9+ float lipoBatteryCharge (uint8_t cells, float voltage);
1310
14- APB::PowerMonitor::PowerMonitor () : d{std::make_shared<Private>()}{
11+ APB::PowerMonitor &APB ::PowerMonitor::Instance = * new APB::PowerMonitor();
1512
13+ APB::PowerMonitor::PowerMonitor () {
1614}
1715
1816APB::PowerMonitor::~PowerMonitor () {
19-
20- }
21-
22- APB::PowerMonitor::Status APB::PowerMonitor::status () const {
23- return d->status ;
2417}
2518
2619void APB::PowerMonitor::setup (Scheduler &scheduler) {
27- d-> status .initialised = d-> ina219 .begin ();
28- if (d-> status .initialised ) {
20+ _status .initialised = _ina219 .begin ();
21+ if (_status .initialised ) {
2922 Log.infoln (" Powermonitor initialised: INA219 with address 0x%x" , APB_INA1219_ADDRESS);
3023
3124
32- bool gainValid = d-> ina219 .setGain (APB_POWER_INA219_GAIN);
33- bool voltageRangeValid = d-> ina219 .setBusVoltageRange (APB_POWER_INA219_VOLTAGE_RANGE);
34- bool shuntValid = d-> ina219 .setMaxCurrentShunt (APB_POWER_MAX_CURRENT_AMPS, APB_POWER_SHUNT_OHMS);
25+ bool gainValid = _ina219 .setGain (APB_POWER_INA219_GAIN);
26+ bool voltageRangeValid = _ina219 .setBusVoltageRange (APB_POWER_INA219_VOLTAGE_RANGE);
27+ bool shuntValid = _ina219 .setMaxCurrentShunt (APB_POWER_MAX_CURRENT_AMPS, APB_POWER_SHUNT_OHMS);
3528
3629 Log.infoln (" Powermonitor settings: valid=%d, %d milliamp max (%d amps), shunt resistor: %d milliohms" ,
3730 shuntValid && voltageRangeValid && gainValid,
38- static_cast <int >(d-> ina219 .getMaxCurrent () * 1000.0 ),
39- static_cast <int >(d-> ina219 .getMaxCurrent ()),
40- static_cast <int >(d-> ina219 .getShunt () * 1000.0 )
31+ static_cast <int >(_ina219 .getMaxCurrent () * 1000.0 ),
32+ static_cast <int >(_ina219 .getMaxCurrent ()),
33+ static_cast <int >(_ina219 .getShunt () * 1000.0 )
4134 );
4235
43- d->loopTask .set (1000 , TASK_FOREVER, [this ](){
44- d->status .busVoltage = d->ina219 .getBusVoltage ();
45- d->status .current = d->ina219 .getCurrent ();
46- d->status .power = d->ina219 .getPower ();
47- d->status .shuntVoltage = d->ina219 .getShuntVoltage ();
48- if (d->status .power == 0 && d->status .current == 0 ) {
49- Log.warningln (" Powermonitor: Reporting power as 0. INA status: %d" , d->ina219 .isConnected ());
36+ _loopTask.set (1000 , TASK_FOREVER, [this ](){
37+ _status.busVoltage = _ina219.getBusVoltage ();
38+ _status.current = _ina219.getCurrent ();
39+ _status.power = _ina219.getPower ();
40+ _status.shuntVoltage = _ina219.getShuntVoltage ();
41+ setCharge ();
42+ if (_status.power == 0 && _status.current == 0 ) {
43+ Log.warningln (" Powermonitor: Reporting power as 0. INA status: %d" , _ina219.isConnected ());
5044 }
5145 });
52- scheduler.addTask (d-> loopTask );
53- d-> loopTask .enable ();
46+ scheduler.addTask (_loopTask );
47+ _loopTask .enable ();
5448 } else {
5549 Log.errorln (" Powermonitor failed to initialise INA219 with address 0x%x" , APB_INA1219_ADDRESS);
5650 }
51+ }
52+
53+ void APB::PowerMonitor::setCharge () {
54+ switch (Settings::Instance.powerSource ()) {
55+ case LipoBattery3C:
56+ _status.charge = lipoBatteryCharge (3 , _status.busVoltage );
57+ break ;
58+ default :
59+ _status.charge = 100 .0f ;
60+ }
61+ }
62+
63+
64+ float lipoBatteryCharge (uint8_t cells, float voltage) {
65+ float singleCellVoltage = voltage / static_cast <float >(cells);
66+ using RefPair = std::pair<float , float >;
67+ static const std::array<RefPair, 20 > chargeMap{{
68+ {95 , 4.15 },
69+ {90 , 4.11 },
70+ {85 , 4.08 },
71+ {80 , 4.02 },
72+ {75 , 3.98 },
73+ {70 , 3.95 },
74+ {65 , 3.91 },
75+ {60 , 3.87 },
76+ {55 , 3.85 },
77+ {50 , 3.84 },
78+ {45 , 3.82 },
79+ {40 , 3.8 },
80+ {35 , 3.79 },
81+ {30 , 3.77 },
82+ {25 , 3.75 },
83+ {20 , 3.73 },
84+ {15 , 3.71 },
85+ {10 , 3.69 },
86+ { 5 , 3.61 },
87+ { 0 , 3.27 },
88+ }};
89+ const RefPair *found = std::find_if (chargeMap.begin (), chargeMap.end (), [&singleCellVoltage](const RefPair &el){ return singleCellVoltage > el.second ; });
90+ if (found == chargeMap.end ()) {
91+ return 100.0 ;
92+ }
93+ return found->first ;
5794}
0 commit comments