/** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup BatteryModule Battery Module * @brief Measures battery voltage and current * Updates the FlightBatteryState object * @{ * * @file battery.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Module to read the battery Voltage and Current periodically and set alarms appropriately. * * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ /** * Output object: FlightBatteryState * * This module will periodically generate information on the battery state. * * UAVObjects are automatically generated by the UAVObjectGenerator from * the object definition XML file. * * Modules have no API, all communication to other modules is done through UAVObjects. * However modules may use the API exposed by shared libraries. * See the OpenPilot wiki for more details. * http://www.openpilot.org/OpenPilot_Application_Architecture * */ #include "openpilot.h" #include "flightbatterystate.h" #include "flightbatterysettings.h" #include "hwsettings.h" // // Configuration // #define SAMPLE_PERIOD_MS 500 // Private types // Private variables static bool batteryEnabled = false; // THESE COULD BE BETTER AS SOME KIND OF UNION OR STRUCT, BY WHICH 4 BITS ARE USED FOR EACH // PIN VARIABLE, ONE OF WHICH INDICATES SIGN, AND THE OTHER 3 BITS INDICATE POSITION. THIS WILL // WORK FOR QUITE SOMETIME, UNTIL MORE THAN 8 ADC ARE AVAILABLE. EVEN AT THIS POINT, THE STRUCTURE // CAN SIMPLY BE MODIFIED TO SUPPORT 15 ADC PINS, BY USING ALL AVAILABLE BITS. static int8_t voltageADCPin = -1; // ADC pin for voltage static int8_t currentADCPin = -1; // ADC pin for current // Private functions static void onTimer(UAVObjEvent *ev); /** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t BatteryInitialize(void) { #ifdef MODULE_BATTERY_BUILTIN batteryEnabled = true; #else uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; HwSettingsOptionalModulesGet(optionalModules); if ((optionalModules[HWSETTINGS_OPTIONALMODULES_BATTERY] == HWSETTINGS_OPTIONALMODULES_ENABLED)) { batteryEnabled = true; } else { batteryEnabled = false; } #endif uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; HwSettingsADCRoutingArrayGet(adcRouting); // Determine if the battery sensors are routed to ADC pins for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYVOLTAGE) { voltageADCPin = i; } if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYCURRENT) { currentADCPin = i; } } // Don't enable module if no ADC pins are routed to the sensors if (voltageADCPin < 0 && currentADCPin < 0) { batteryEnabled = false; } // Start module if (batteryEnabled) { FlightBatteryStateInitialize(); FlightBatterySettingsInitialize(); static UAVObjEvent ev; memset(&ev, 0, sizeof(UAVObjEvent)); EventPeriodicCallbackCreate(&ev, onTimer, SAMPLE_PERIOD_MS / portTICK_RATE_MS); } return 0; } MODULE_INITCALL(BatteryInitialize, 0); static void onTimer(__attribute__((unused)) UAVObjEvent *ev) { static FlightBatteryStateData flightBatteryData; static FlightBatterySettingsData batterySettings; FlightBatterySettingsGet(&batterySettings); const float dT = SAMPLE_PERIOD_MS / 1000.0f; float energyRemaining; // calculate the battery parameters if (voltageADCPin >= 0) { flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.VoltageZero) * batterySettings.SensorCalibrations.VoltageFactor; // in Volts } else { flightBatteryData.Voltage = 0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } if (currentADCPin >= 0) { flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations.CurrentZero) * batterySettings.SensorCalibrations.CurrentFactor; // in Amps if (flightBatteryData.Current > flightBatteryData.PeakCurrent) { flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps } } else { // If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm flightBatteryData.Current = -0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); // in mAh // Apply a 2 second rise time low-pass filter to average the current float alpha = 1.0f - dT / (dT + 2.0f); flightBatteryData.AvgCurrent = alpha * flightBatteryData.AvgCurrent + (1 - alpha) * flightBatteryData.Current; // in Amps /*The motor could regenerate power. Or we could have solar cells. In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple sign check doesn't catch this.*/ energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh if (batterySettings.Capacity > 0 && flightBatteryData.AvgCurrent > 0) { flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent * 1000.0f)) * 3600.0f; // in Sec } else { flightBatteryData.EstimatedFlightTime = 0; } // generate alarms where needed... if ((flightBatteryData.Voltage <= 0) && (flightBatteryData.Current <= 0)) { // FIXME: There's no guarantee that a floating ADC will give 0. So this // check might fail, even when there's nothing attached. AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR); } else { // FIXME: should make the timer alarms user configurable if (batterySettings.Capacity > 0 && flightBatteryData.EstimatedFlightTime < 30) { AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); } else if (batterySettings.Capacity > 0 && flightBatteryData.EstimatedFlightTime < 120) { AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME); } // FIXME: should make the battery voltage detection dependent on battery type. /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Alarm * batterySettings.NbCells) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * batterySettings.NbCells) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); } } FlightBatteryStateSet(&flightBatteryData); } /** * @} */ /** * @} */