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