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
  65 #ifndef PIOS_ADC_VOLTAGE_PIN
  66 #define PIOS_ADC_VOLTAGE_PIN -1
  67 #endif
  68
  69 #ifndef PIOS_ADC_CURRENT_PIN
  70 #define PIOS_ADC_CURRENT_PIN -1
  71 #endif
  72
  73
  74 // THESE COULD BE BETTER AS SOME KIND OF UNION OR STRUCT, BY WHICH 4 BITS ARE USED FOR EACH
  75 // PIN VARIABLE, ONE OF WHICH INDICATES SIGN, AND THE OTHER 3 BITS INDICATE POSITION. THIS WILL
  76 // WORK FOR QUITE SOMETIME, UNTIL MORE THAN 8 ADC ARE AVAILABLE. EVEN AT THIS POINT, THE STRUCTURE
  77 // CAN SIMPLY BE MODIFIED TO SUPPORT 15 ADC PINS, BY USING ALL AVAILABLE BITS.
  78 static int8_t voltageADCPin = PIOS_ADC_VOLTAGE_PIN; // ADC pin for voltage
  79 static int8_t currentADCPin = PIOS_ADC_CURRENT_PIN; // ADC pin for current
  80
  81 // Private functions
  82 static void onTimer(UAVObjEvent *ev);
  83 static void GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData);
  84
  85 /**
  86  * Initialise the module, called on startup
  87  * \returns 0 on success or -1 if initialisation failed
  88  */
  89 int32_t BatteryInitialize(void)
  90 {
  91     bool batteryEnabled;
  92
  93     HwSettingsOptionalModulesData optionalModules;
  94
  95     HwSettingsOptionalModulesGet(&optionalModules);
  96
  97 #ifdef MODULE_BATTERY_BUILTIN
  98     batteryEnabled = true;
  99     optionalModules.Battery = HWSETTINGS_OPTIONALMODULES_ENABLED;
 100     HwSettingsOptionalModulesSet(&optionalModules);
 101 #else
 102     if (optionalModules.Battery == HWSETTINGS_OPTIONALMODULES_ENABLED) {
 103         batteryEnabled = true;
 104     } else {
 105         batteryEnabled = false;
 106     }
 107 #endif
 108
 109     uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
 110     HwSettingsADCRoutingArrayGet(adcRouting);
 111
 112     // Determine if the battery sensors are routed to ADC pins
 113     for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
 114         if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYVOLTAGE) {
 115             voltageADCPin = i;
 116         }
 117         if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYCURRENT) {
 118             currentADCPin = i;
 119         }
 120     }
 121
 122     // Don't enable module if no ADC pins are routed to the sensors
 123     if (voltageADCPin < 0 && currentADCPin < 0) {
 124         batteryEnabled = false;
 125     }
 126
 127     // Start module
 128     if (batteryEnabled) {
 129         FlightBatteryStateInitialize();
 130         SystemStatsInitialize();
 131
 132         static UAVObjEvent ev;
 133
 134         memset(&ev, 0, sizeof(UAVObjEvent));
 135         EventPeriodicCallbackCreate(&ev, onTimer, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
 136     }
 137
 138     return 0;
 139 }
 140
 141 MODULE_INITCALL(BatteryInitialize, 0);
 142 static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
 143 {
 144     static FlightBatterySettingsData batterySettings;
 145     static FlightBatteryStateData flightBatteryData;
 146
 147     FlightBatterySettingsGet(&batterySettings);
 148     FlightBatteryStateGet(&flightBatteryData);
 149
 150     const float dT = SAMPLE_PERIOD_MS / 1000.0f;
 151     float energyRemaining;
 152
 153     // Reset ConsumedEnergy counter
 154     if (batterySettings.ResetConsumedEnergy) {
 155         flightBatteryData.ConsumedEnergy    = 0;
 156         batterySettings.ResetConsumedEnergy = false;
 157         FlightBatterySettingsSet(&batterySettings);
 158     }
 159 #ifdef PIOS_INCLUDE_ADC
 160     // calculate the battery parameters
 161     if (voltageADCPin >= 0) {
 162         flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.VoltageZero) * batterySettings.SensorCalibrations.VoltageFactor; // in Volts
 163     } else {
 164         flightBatteryData.Voltage = 0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
 165     }
 166 #else
 167     flightBatteryData.Voltage = 0;
 168 #endif /* PIOS_INCLUDE_ADC */
 169     // voltage available: get the number of cells if possible, desired and not armed
 170     GetNbCells(&batterySettings, &flightBatteryData);
 171 #ifdef PIOS_INCLUDE_ADC
 172     // ad a plausibility check: zero voltage => zero current
 173     if (currentADCPin >= 0 && flightBatteryData.Voltage > 0.f) {
 174         flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations.CurrentZero) * batterySettings.SensorCalibrations.CurrentFactor; // in Amps
 175         if (flightBatteryData.Current > flightBatteryData.PeakCurrent) {
 176             flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps
 177         }
 178     } else { // If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm
 179         flightBatteryData.Current = -0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
 180     }
 181 #else
 182     flightBatteryData.Current = -0;
 183 #endif /* PIOS_INCLUDE_ADC */
 184
 185     // For safety reasons consider only positive currents in energy comsumption, i.e. no charging up.
 186     // necesary when sensor are not perfectly calibrated
 187     if (flightBatteryData.Current > 0) {
 188         flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); // in mAh
 189     }
 190
 191     // Apply a 2 second rise time low-pass filter to average the current
 192     float alpha = 1.0f - dT / (dT + 2.0f);
 193     flightBatteryData.AvgCurrent = alpha * flightBatteryData.AvgCurrent + (1 - alpha) * flightBatteryData.Current; // in Amps
 194
 195     /*The motor could regenerate power. Or we could have solar cells.
 196        In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then
 197        it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple
 198        sign check doesn't catch this.*/
 199     energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh
 200     if (batterySettings.Capacity > 0 && flightBatteryData.AvgCurrent > 0) {
 201         flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent * 1000.0f)) * 3600.0f; // in Sec
 202     } else {
 203         flightBatteryData.EstimatedFlightTime = 0;
 204     }
 205
 206     // generate alarms where needed...
 207     if ((flightBatteryData.Voltage <= 0) && (flightBatteryData.Current <= 0)) {
 208         // FIXME: There's no guarantee that a floating ADC will give 0. So this
 209         // check might fail, even when there's nothing attached.
 210         AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);
 211         AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR);
 212     } else {
 213         // FIXME: should make the timer alarms user configurable
 214         if (batterySettings.Capacity > 0 && flightBatteryData.EstimatedFlightTime < 30) {
 215             AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL);
 216         } else if (batterySettings.Capacity > 0 && flightBatteryData.EstimatedFlightTime < 120) {
 217             AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING);
 218         } else {
 219             AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME);
 220         }
 221
 222         // FIXME: should make the battery voltage detection dependent on battery type.
 223         /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/
 224         if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Critical * flightBatteryData.NbCells) {
 225             AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);
 226         } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * flightBatteryData.NbCells) {
 227             AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
 228         } else {
 229             AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
 230         }
 231     }
 232
 233     FlightBatteryStateSet(&flightBatteryData);
 234 }
 235
 236
 237 static void GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData)
 238 {
 239     // get flight status to check for armed
 240     uint8_t armed = 0;
 241     static bool detected = false;
 242
 243     // prevent the cell number to change once the board is armed at least once
 244     if (detected) {
 245         return;
 246     }
 247
 248     FlightStatusArmedGet(&armed);
 249
 250     // check only if not armed
 251     if (armed == FLIGHTSTATUS_ARMED_ARMED) {
 252         detected = true;
 253         return;
 254     }
 255
 256     // prescribed number of cells?
 257     if (batterySettings->NbCells != 0) {
 258         flightBatteryData->NbCells = batterySettings->NbCells;
 259         flightBatteryData->NbCellsAutodetected = 0;
 260         return;
 261     }
 262
 263     // plausibility check
 264     if (flightBatteryData->Voltage <= 0.5f) {
 265         // cannot detect number of cells
 266         flightBatteryData->NbCellsAutodetected = 0;
 267         return;
 268     }
 269
 270     float voltageMin = 0.f, voltageMax = 0.f;
 271
 272     // Cell type specific values
 273     // TODO: could be implemented as constant arrays indexed by cellType
 274     // or could be part of the UAVObject definition
 275     switch (batterySettings->Type) {
 276     case FLIGHTBATTERYSETTINGS_TYPE_LIPO:
 277     case FLIGHTBATTERYSETTINGS_TYPE_LICO:
 278         voltageMin = 3.6f;
 279         voltageMax = 4.2f;
 280         break;
 281     case FLIGHTBATTERYSETTINGS_TYPE_LIHV:
 282         voltageMin = 3.6f;
 283         voltageMax = 4.35f;
 284         break;
 285     case FLIGHTBATTERYSETTINGS_TYPE_A123:
 286         voltageMin = 2.01f;
 287         voltageMax = 3.59f;
 288         break;
 289     case FLIGHTBATTERYSETTINGS_TYPE_LIFESO4:
 290     default:
 291         flightBatteryData->NbCellsAutodetected = 0;
 292         return;
 293     }
 294
 295     // uniquely measurable under any condition iff n * voltageMax < (n+1) * voltageMin
 296     // or n < voltageMin / (voltageMax-voltageMin)
 297     // weaken condition by setting n <= voltageMin / (voltageMax-voltageMin) and
 298     // checking for v <= voltageMin * voltageMax / (voltageMax-voltageMin)
 299     if (flightBatteryData->Voltage > voltageMin * voltageMax / (voltageMax - voltageMin)) {
 300         flightBatteryData->NbCellsAutodetected = 0;
 301         return;
 302     }
 303
 304     // Prevent the battery discharging on the ground to change the detected number of cells:
 305     // Detection is enabled in the first 60 seconds from powerup
 306     uint32_t flightTime;
 307     SystemStatsFlightTimeGet(&flightTime);
 308     if (flightTime > DETECTION_TIMEFRAME) {
 309         detected = true;
 310     }
 311
 312     flightBatteryData->NbCells = (int8_t)(flightBatteryData->Voltage / voltageMin);
 313     flightBatteryData->NbCellsAutodetected = 1;
 314 }
 315
 316 /**
 317  * @}
 318  */
 319
 320 /**
 321  * @}
 322  */