flight/modules/Battery/battery.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup BatteryModule Battery Module
   6  * @brief Measures battery voltage and current
   7  * Updates the FlightBatteryState object
   8  * @{
   9  *
  10  * @file       battery.c
  11  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  12  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  13  * @brief      Module to read the battery Voltage and Current periodically and set alarms appropriately.
  14  *
  15  * @see        The GNU Public License (GPL) Version 3
  16  *
  17  *****************************************************************************/
  18 /*
  19  * This program is free software; you can redistribute it and/or modify
  20  * it under the terms of the GNU General Public License as published by
  21  * the Free Software Foundation; either version 3 of the License, or
  22  * (at your option) any later version.
  23  *
  24  * This program is distributed in the hope that it will be useful, but
  25  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  26  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  27  * for more details.
  28  *
  29  * You should have received a copy of the GNU General Public License along
  30  * with this program; if not, write to the Free Software Foundation, Inc.,
  31  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  32  */
  33
  34 /**
  35  * Output object: FlightBatteryState
  36  *
  37  * This module will periodically generate information on the battery state.
  38  *
  39  * UAVObjects are automatically generated by the UAVObjectGenerator from
  40  * the object definition XML file.
  41  *
  42  * Modules have no API, all communication to other modules is done through UAVObjects.
  43  * However modules may use the API exposed by shared libraries.
  44  * See the OpenPilot wiki for more details.
  45  * http://www.openpilot.org/OpenPilot_Application_Architecture
  46  *
  47  */
  48
  49 #include "openpilot.h"
  50
  51 #include "flightstatus.h"
  52 #include "flightbatterystate.h"
  53 #include "flightbatterysettings.h"
  54 #include "hwsettings.h"
  55 #include "systemstats.h"
  56
  57 //
  58 // Configuration
  59 //
  60 #define SAMPLE_PERIOD_MS    500
  61
  62 // Time since power on the cells detection is active
  63 #define DETECTION_TIMEFRAME 60000
  64 // Private types
  65
  66 // Private variables
  67 static bool batteryEnabled = false;
  68
  69 // THESE COULD BE BETTER AS SOME KIND OF UNION OR STRUCT, BY WHICH 4 BITS ARE USED FOR EACH
  70 // PIN VARIABLE, ONE OF WHICH INDICATES SIGN, AND THE OTHER 3 BITS INDICATE POSITION. THIS WILL
  71 // WORK FOR QUITE SOMETIME, UNTIL MORE THAN 8 ADC ARE AVAILABLE. EVEN AT THIS POINT, THE STRUCTURE
  72 // CAN SIMPLY BE MODIFIED TO SUPPORT 15 ADC PINS, BY USING ALL AVAILABLE BITS.
  73 static int8_t voltageADCPin = -1; // ADC pin for voltage
  74 static int8_t currentADCPin = -1; // ADC pin for current
  75
  76 // Private functions
  77 static void onTimer(UAVObjEvent *ev);
  78 static void GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData);
  79
  80 /**
  81  * Initialise the module, called on startup
  82  * \returns 0 on success or -1 if initialisation failed
  83  */
  84 int32_t BatteryInitialize(void)
  85 {
  86 #ifdef MODULE_BATTERY_BUILTIN
  87     batteryEnabled = true;
  88 #else
  89     HwSettingsInitialize();
  90     uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
  91
  92     HwSettingsOptionalModulesGet(optionalModules);
  93
  94     if ((optionalModules[HWSETTINGS_OPTIONALMODULES_BATTERY] == HWSETTINGS_OPTIONALMODULES_ENABLED)) {
  95         batteryEnabled = true;
  96     } else {
  97         batteryEnabled = false;
  98     }
  99 #endif
 100
 101     uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
 102     HwSettingsADCRoutingArrayGet(adcRouting);
 103
 104     // Determine if the battery sensors are routed to ADC pins
 105     for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
 106         if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYVOLTAGE) {
 107             voltageADCPin = i;
 108         }
 109         if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYCURRENT) {
 110             currentADCPin = i;
 111         }
 112     }
 113
 114     // Don't enable module if no ADC pins are routed to the sensors
 115     if (voltageADCPin < 0 && currentADCPin < 0) {
 116         batteryEnabled = false;
 117     }
 118
 119     // Start module
 120     if (batteryEnabled) {
 121         FlightBatteryStateInitialize();
 122         FlightBatterySettingsInitialize();
 123         SystemStatsInitialize();
 124
 125         static UAVObjEvent ev;
 126
 127         memset(&ev, 0, sizeof(UAVObjEvent));
 128         EventPeriodicCallbackCreate(&ev, onTimer, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
 129     }
 130
 131     return 0;
 132 }
 133
 134 MODULE_INITCALL(BatteryInitialize, 0);
 135 static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
 136 {
 137     static FlightBatterySettingsData batterySettings;
 138     static FlightBatteryStateData flightBatteryData;
 139
 140     FlightBatterySettingsGet(&batterySettings);
 141     FlightBatteryStateGet(&flightBatteryData);
 142
 143     const float dT = SAMPLE_PERIOD_MS / 1000.0f;
 144     float energyRemaining;
 145
 146     // Reset ConsumedEnergy counter
 147     if (batterySettings.ResetConsumedEnergy) {
 148         flightBatteryData.ConsumedEnergy    = 0;
 149         batterySettings.ResetConsumedEnergy = false;
 150         FlightBatterySettingsSet(&batterySettings);
 151     }
 152
 153     // calculate the battery parameters
 154     if (voltageADCPin >= 0) {
 155         flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.VoltageZero) * batterySettings.SensorCalibrations.VoltageFactor; // in Volts
 156     } else {
 157         flightBatteryData.Voltage = 0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
 158     }
 159
 160     // voltage available: get the number of cells if possible, desired and not armed
 161     GetNbCells(&batterySettings, &flightBatteryData);
 162
 163     // ad a plausibility check: zero voltage => zero current
 164     if (currentADCPin >= 0 && flightBatteryData.Voltage > 0.f) {
 165         flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations.CurrentZero) * batterySettings.SensorCalibrations.CurrentFactor; // in Amps
 166         if (flightBatteryData.Current > flightBatteryData.PeakCurrent) {
 167             flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps
 168         }
 169     } else { // If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm
 170         flightBatteryData.Current = -0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
 171     }
 172
 173     // For safety reasons consider only positive currents in energy comsumption, i.e. no charging up.
 174     // necesary when sensor are not perfectly calibrated
 175     if (flightBatteryData.Current > 0) {
 176         flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); // in mAh
 177     }
 178
 179     // Apply a 2 second rise time low-pass filter to average the current
 180     float alpha = 1.0f - dT / (dT + 2.0f);
 181     flightBatteryData.AvgCurrent = alpha * flightBatteryData.AvgCurrent + (1 - alpha) * flightBatteryData.Current; // in Amps
 182
 183     /*The motor could regenerate power. Or we could have solar cells.
 184        In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then
 185        it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple
 186        sign check doesn't catch this.*/
 187     energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh
 188     if (batterySettings.Capacity > 0 && flightBatteryData.AvgCurrent > 0) {
 189         flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent * 1000.0f)) * 3600.0f; // in Sec
 190     } else {
 191         flightBatteryData.EstimatedFlightTime = 0;
 192     }
 193
 194     // generate alarms where needed...
 195     if ((flightBatteryData.Voltage <= 0) && (flightBatteryData.Current <= 0)) {
 196         // FIXME: There's no guarantee that a floating ADC will give 0. So this
 197         // check might fail, even when there's nothing attached.
 198         AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);
 199         AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR);
 200     } else {
 201         // FIXME: should make the timer alarms user configurable
 202         if (batterySettings.Capacity > 0 && flightBatteryData.EstimatedFlightTime < 30) {
 203             AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL);
 204         } else if (batterySettings.Capacity > 0 && flightBatteryData.EstimatedFlightTime < 120) {
 205             AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING);
 206         } else {
 207             AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME);
 208         }
 209
 210         // FIXME: should make the battery voltage detection dependent on battery type.
 211         /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/
 212         if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Critical * flightBatteryData.NbCells) {
 213             AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);
 214         } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * flightBatteryData.NbCells) {
 215             AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
 216         } else {
 217             AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
 218         }
 219     }
 220
 221     FlightBatteryStateSet(&flightBatteryData);
 222 }
 223
 224
 225 static void GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData)
 226 {
 227     // get flight status to check for armed
 228     uint8_t armed = 0;
 229     static bool detected = false;
 230
 231     // prevent the cell number to change once the board is armed at least once
 232     if (detected) {
 233         return;
 234     }
 235
 236     FlightStatusArmedGet(&armed);
 237
 238     // check only if not armed
 239     if (armed == FLIGHTSTATUS_ARMED_ARMED) {
 240         detected = true;
 241         return;
 242     }
 243
 244     // prescribed number of cells?
 245     if (batterySettings->NbCells != 0) {
 246         flightBatteryData->NbCells = batterySettings->NbCells;
 247         flightBatteryData->NbCellsAutodetected = 0;
 248         return;
 249     }
 250
 251     // plausibility check
 252     if (flightBatteryData->Voltage <= 0.5f) {
 253         // cannot detect number of cells
 254         flightBatteryData->NbCellsAutodetected = 0;
 255         return;
 256     }
 257
 258     float voltageMin = 0.f, voltageMax = 0.f;
 259
 260     // Cell type specific values
 261     // TODO: could be implemented as constant arrays indexed by cellType
 262     // or could be part of the UAVObject definition
 263     switch (batterySettings->Type) {
 264     case FLIGHTBATTERYSETTINGS_TYPE_LIPO:
 265     case FLIGHTBATTERYSETTINGS_TYPE_LICO:
 266         voltageMin = 3.6f;
 267         voltageMax = 4.2f;
 268         break;
 269     case FLIGHTBATTERYSETTINGS_TYPE_LIHV:
 270         voltageMin = 3.6f;
 271         voltageMax = 4.35f;
 272         break;
 273     case FLIGHTBATTERYSETTINGS_TYPE_A123:
 274         voltageMin = 2.01f;
 275         voltageMax = 3.59f;
 276         break;
 277     case FLIGHTBATTERYSETTINGS_TYPE_LIFESO4:
 278     default:
 279         flightBatteryData->NbCellsAutodetected = 0;
 280         return;
 281     }
 282
 283     // uniquely measurable under any condition iff n * voltageMax < (n+1) * voltageMin
 284     // or n < voltageMin / (voltageMax-voltageMin)
 285     // weaken condition by setting n <= voltageMin / (voltageMax-voltageMin) and
 286     // checking for v <= voltageMin * voltageMax / (voltageMax-voltageMin)
 287     if (flightBatteryData->Voltage > voltageMin * voltageMax / (voltageMax - voltageMin)) {
 288         flightBatteryData->NbCellsAutodetected = 0;
 289         return;
 290     }
 291
 292     // Prevent the battery discharging on the ground to change the detected number of cells:
 293     // Detection is enabled in the first 60 seconds from powerup
 294     uint32_t flightTime;
 295     SystemStatsFlightTimeGet(&flightTime);
 296     if (flightTime > DETECTION_TIMEFRAME) {
 297         detected = true;
 298     }
 299
 300     flightBatteryData->NbCells = (int8_t)(flightBatteryData->Voltage / voltageMin);
 301     flightBatteryData->NbCellsAutodetected = 1;
 302 }
 303
 304 /**
 305  * @}
 306  */
 307
 308 /**
 309  * @}
 310  */