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     // calculate the battery parameters
 142     if (voltageADCPin >= 0) {
 143         flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.VoltageZero) * batterySettings.SensorCalibrations.VoltageFactor; // in Volts
 144     } else {
 145         flightBatteryData.Voltage = 0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
 146     }
 147
 148     // voltage available: get the number of cells if possible, desired and not armed
 149     GetNbCells(&batterySettings, &flightBatteryData);
 150
 151     // ad a plausibility check: zero voltage => zero current
 152     if (currentADCPin >= 0 && flightBatteryData.Voltage > 0.f) {
 153         flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations.CurrentZero) * batterySettings.SensorCalibrations.CurrentFactor; // in Amps
 154         if (flightBatteryData.Current > flightBatteryData.PeakCurrent) {
 155             flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps
 156         }
 157     } else { // If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm
 158         flightBatteryData.Current = -0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
 159     }
 160
 161     // For safety reasons consider only positive currents in energy comsumption, i.e. no charging up.
 162     // necesary when sensor are not perfectly calibrated
 163     if (flightBatteryData.Current > 0) {
 164         flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); // in mAh
 165     }
 166
 167     // Apply a 2 second rise time low-pass filter to average the current
 168     float alpha = 1.0f - dT / (dT + 2.0f);
 169     flightBatteryData.AvgCurrent = alpha * flightBatteryData.AvgCurrent + (1 - alpha) * flightBatteryData.Current; // in Amps
 170
 171     /*The motor could regenerate power. Or we could have solar cells.
 172        In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then
 173        it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple
 174        sign check doesn't catch this.*/
 175     energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh
 176     if (batterySettings.Capacity > 0 && flightBatteryData.AvgCurrent > 0) {
 177         flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent * 1000.0f)) * 3600.0f; // in Sec
 178     } else {
 179         flightBatteryData.EstimatedFlightTime = 0;
 180     }
 181
 182     // generate alarms where needed...
 183     if ((flightBatteryData.Voltage <= 0) && (flightBatteryData.Current <= 0)) {
 184         // FIXME: There's no guarantee that a floating ADC will give 0. So this
 185         // check might fail, even when there's nothing attached.
 186         AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);
 187         AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR);
 188     } else {
 189         // FIXME: should make the timer alarms user configurable
 190         if (batterySettings.Capacity > 0 && flightBatteryData.EstimatedFlightTime < 30) {
 191             AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL);
 192         } else if (batterySettings.Capacity > 0 && flightBatteryData.EstimatedFlightTime < 120) {
 193             AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING);
 194         } else {
 195             AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME);
 196         }
 197
 198         // FIXME: should make the battery voltage detection dependent on battery type.
 199         /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/
 200         if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Alarm * flightBatteryData.NbCells) {
 201             AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);
 202         } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * flightBatteryData.NbCells) {
 203             AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
 204         } else {
 205             AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
 206         }
 207     }
 208
 209     FlightBatteryStateSet(&flightBatteryData);
 210 }
 211
 212
 213 static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData)
 214 {
 215     // get flight status to check for armed
 216     uint8_t armed = 0;
 217
 218     FlightStatusArmedGet(&armed);
 219
 220
 221     // check only if not armed
 222     if (armed == FLIGHTSTATUS_ARMED_ARMED) {
 223         return -2;
 224     }
 225
 226     // prescribed number of cells?
 227     if (batterySettings->NbCells != 0) {
 228         flightBatteryData->NbCells = batterySettings->NbCells;
 229         flightBatteryData->NbCellsAutodetected = 0;
 230         return -3;
 231     }
 232
 233     // plausibility check
 234     if (flightBatteryData->Voltage <= 0.5f) {
 235         // cannot detect number of cells
 236         flightBatteryData->NbCellsAutodetected = 0;
 237         return -1;
 238     }
 239
 240     float voltageMin = 0.f, voltageMax = 0.f;
 241
 242     // Cell type specific values
 243     // TODO: could be implemented as constant arrays indexed by cellType
 244     // or could be part of the UAVObject definition
 245     switch (batterySettings->Type) {
 246     case FLIGHTBATTERYSETTINGS_TYPE_LIPO:
 247     case FLIGHTBATTERYSETTINGS_TYPE_LICO:
 248         voltageMin = 3.6f;
 249         voltageMax = 4.2f;
 250         break;
 251     case FLIGHTBATTERYSETTINGS_TYPE_A123:
 252         voltageMin = 2.01f;
 253         voltageMax = 3.59f;
 254         break;
 255     case FLIGHTBATTERYSETTINGS_TYPE_LIFESO4:
 256     default:
 257         flightBatteryData->NbCellsAutodetected = 0;
 258         return -1;
 259     }
 260
 261     // uniquely measurable under any condition iff n * voltageMax < (n+1) * voltageMin
 262     // or n < voltageMin / (voltageMax-voltageMin)
 263     // weaken condition by setting n <= voltageMin / (voltageMax-voltageMin) and
 264     // checking for v <= voltageMin * voltageMax / (voltageMax-voltageMin)
 265     if (flightBatteryData->Voltage > voltageMin * voltageMax / (voltageMax - voltageMin)) {
 266         flightBatteryData->NbCellsAutodetected = 0;
 267         return -1;
 268     }
 269
 270
 271     flightBatteryData->NbCells = (int8_t)(flightBatteryData->Voltage / voltageMin);
 272     flightBatteryData->NbCellsAutodetected = 1;
 273
 274     return flightBatteryData->NbCells;
 275 }
 276
 277 /**
 278  * @}
 279  */
 280
 281 /**
 282  * @}
 283  */