2010-09-27 09:28:34 +02:00
/**
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* @ addtogroup OpenPilotModules OpenPilot Modules
2011-01-03 02:23:04 +01:00
* @ {
2010-09-27 09:28:34 +02:00
* @ addtogroup BatteryModule Battery Module
* @ brief Measures battery voltage and current
* Updates the FlightBatteryState object
2011-01-03 02:23:04 +01:00
* @ {
2010-09-27 09:28:34 +02:00
*
* @ file battery . c
2016-11-04 00:27:18 +01:00
* @ author The LibrePilot Project , http : //www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team , http : //www.openpilot.org Copyright (C) 2010.
2011-01-17 05:02:37 +01:00
* @ brief Module to read the battery Voltage and Current periodically and set alarms appropriately .
2010-09-27 09:28:34 +02:00
*
* @ 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"
2014-04-10 17:28:48 +02:00
# include "flightstatus.h"
2010-09-27 09:28:34 +02:00
# include "flightbatterystate.h"
2011-03-19 19:45:40 +01:00
# include "flightbatterysettings.h"
2012-06-04 17:31:45 +02:00
# include "hwsettings.h"
2017-02-20 22:29:43 +01:00
# include "systemstats.h"
2010-09-27 09:28:34 +02:00
//
// Configuration
//
2017-02-20 22:29:43 +01:00
# define SAMPLE_PERIOD_MS 500
// Time since power on the cells detection is active
# define DETECTION_TIMEFRAME 60000
2010-09-27 09:28:34 +02:00
// Private types
// Private variables
2012-06-04 17:31:45 +02:00
static bool batteryEnabled = false ;
2010-09-27 09:28:34 +02:00
2017-05-30 10:57:25 +02:00
# ifndef PIOS_ADC_VOLTAGE_PIN
# define PIOS_ADC_VOLTAGE_PIN -1
# endif
# ifndef PIOS_ADC_CURRENT_PIN
# define PIOS_ADC_CURRENT_PIN -1
# endif
2013-05-19 16:37:30 +02:00
// 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.
2017-05-30 10:57:25 +02:00
static int8_t voltageADCPin = PIOS_ADC_VOLTAGE_PIN ; // ADC pin for voltage
static int8_t currentADCPin = PIOS_ADC_CURRENT_PIN ; // ADC pin for current
2012-07-08 13:16:05 +02:00
2010-09-27 09:28:34 +02:00
// Private functions
2013-05-19 16:37:30 +02:00
static void onTimer ( UAVObjEvent * ev ) ;
2017-02-20 22:29:43 +01:00
static void GetNbCells ( const FlightBatterySettingsData * batterySettings , FlightBatteryStateData * flightBatteryData ) ;
2010-09-27 09:28:34 +02:00
/**
* Initialise the module , called on startup
* \ returns 0 on success or - 1 if initialisation failed
*/
int32_t BatteryInitialize ( void )
{
2012-06-04 17:31:45 +02:00
# ifdef MODULE_BATTERY_BUILTIN
2013-05-19 16:37:30 +02:00
batteryEnabled = true ;
2012-06-04 17:31:45 +02:00
# else
2017-05-22 15:08:04 +02:00
HwSettingsOptionalModulesData optionalModules ;
HwSettingsOptionalModulesGet ( & optionalModules ) ;
2012-06-04 17:31:45 +02:00
2017-05-22 15:08:04 +02:00
if ( optionalModules . Battery = = HWSETTINGS_OPTIONALMODULES_ENABLED ) {
2013-05-19 16:37:30 +02:00
batteryEnabled = true ;
} else {
batteryEnabled = false ;
}
2012-07-08 23:13:05 +02:00
# endif
2013-05-19 16:37:30 +02:00
uint8_t adcRouting [ HWSETTINGS_ADCROUTING_NUMELEM ] ;
2013-09-01 13:23:20 +02:00
HwSettingsADCRoutingArrayGet ( adcRouting ) ;
2013-05-19 16:37:30 +02:00
// 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 ( ) ;
2017-02-20 22:29:43 +01:00
SystemStatsInitialize ( ) ;
2014-04-10 17:28:48 +02:00
2013-05-19 16:37:30 +02:00
static UAVObjEvent ev ;
memset ( & ev , 0 , sizeof ( UAVObjEvent ) ) ;
EventPeriodicCallbackCreate ( & ev , onTimer , SAMPLE_PERIOD_MS / portTICK_RATE_MS ) ;
}
return 0 ;
2010-09-27 09:28:34 +02:00
}
2013-06-04 05:37:40 +02:00
MODULE_INITCALL ( BatteryInitialize , 0 ) ;
2013-05-19 16:37:30 +02:00
static void onTimer ( __attribute__ ( ( unused ) ) UAVObjEvent * ev )
2010-09-27 09:28:34 +02:00
{
2013-05-27 02:12:39 +02:00
static FlightBatterySettingsData batterySettings ;
2014-04-10 17:28:48 +02:00
static FlightBatteryStateData flightBatteryData ;
2013-05-19 16:37:30 +02:00
FlightBatterySettingsGet ( & batterySettings ) ;
2014-04-10 17:28:48 +02:00
FlightBatteryStateGet ( & flightBatteryData ) ;
2013-05-19 16:37:30 +02:00
2013-05-27 02:12:39 +02:00
const float dT = SAMPLE_PERIOD_MS / 1000.0f ;
2013-05-19 16:37:30 +02:00
float energyRemaining ;
2015-04-04 22:34:22 +02:00
// Reset ConsumedEnergy counter
if ( batterySettings . ResetConsumedEnergy ) {
2015-04-20 14:52:13 +02:00
flightBatteryData . ConsumedEnergy = 0 ;
2015-04-04 22:34:22 +02:00
batterySettings . ResetConsumedEnergy = false ;
FlightBatterySettingsSet ( & batterySettings ) ;
}
2017-05-22 15:08:04 +02:00
# ifdef PIOS_INCLUDE_ADC
2013-05-19 16:37:30 +02:00
// calculate the battery parameters
if ( voltageADCPin > = 0 ) {
2013-09-01 12:10:55 +02:00
flightBatteryData . Voltage = ( PIOS_ADC_PinGetVolt ( voltageADCPin ) - batterySettings . SensorCalibrations . VoltageZero ) * batterySettings . SensorCalibrations . VoltageFactor ; // in Volts
2013-05-19 16:37:30 +02:00
} else {
2013-05-27 02:12:39 +02:00
flightBatteryData . Voltage = 0 ; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
2013-05-19 16:37:30 +02:00
}
2017-05-22 15:08:04 +02:00
# else
flightBatteryData . Voltage = 0 ;
# endif /* PIOS_INCLUDE_ADC */
2014-04-10 17:28:48 +02:00
// voltage available: get the number of cells if possible, desired and not armed
GetNbCells ( & batterySettings , & flightBatteryData ) ;
2017-05-22 15:08:04 +02:00
# ifdef PIOS_INCLUDE_ADC
2014-04-10 17:28:48 +02:00
// ad a plausibility check: zero voltage => zero current
2014-04-13 14:07:16 +02:00
if ( currentADCPin > = 0 & & flightBatteryData . Voltage > 0.f ) {
2013-09-01 12:10:55 +02:00
flightBatteryData . Current = ( PIOS_ADC_PinGetVolt ( currentADCPin ) - batterySettings . SensorCalibrations . CurrentZero ) * batterySettings . SensorCalibrations . CurrentFactor ; // in Amps
2013-05-19 16:37:30 +02:00
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
2013-05-27 02:12:39 +02:00
flightBatteryData . Current = - 0 ; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
2013-05-19 16:37:30 +02:00
}
2017-05-22 15:08:04 +02:00
# else
flightBatteryData . Current = - 0 ;
# endif /* PIOS_INCLUDE_ADC */
2013-05-19 16:37:30 +02:00
2014-04-10 17:28:48 +02:00
// For safety reasons consider only positive currents in energy comsumption, i.e. no charging up.
// necesary when sensor are not perfectly calibrated
if ( flightBatteryData . Current > 0 ) {
flightBatteryData . ConsumedEnergy + = ( flightBatteryData . Current * dT * 1000.0f / 3600.0f ) ; // in mAh
}
2013-05-19 16:37:30 +02:00
// 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
2013-07-15 09:47:06 +02:00
if ( batterySettings . Capacity > 0 & & flightBatteryData . AvgCurrent > 0 ) {
2013-05-19 16:37:30 +02:00
flightBatteryData . EstimatedFlightTime = ( energyRemaining / ( flightBatteryData . AvgCurrent * 1000.0f ) ) * 3600.0f ; // in Sec
} else {
2013-07-15 09:47:06 +02:00
flightBatteryData . EstimatedFlightTime = 0 ;
2013-05-19 16:37:30 +02:00
}
// 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
2013-07-15 09:47:06 +02:00
if ( batterySettings . Capacity > 0 & & flightBatteryData . EstimatedFlightTime < 30 ) {
2013-05-19 16:37:30 +02:00
AlarmsSet ( SYSTEMALARMS_ALARM_FLIGHTTIME , SYSTEMALARMS_ALARM_CRITICAL ) ;
2013-07-15 09:47:06 +02:00
} else if ( batterySettings . Capacity > 0 & & flightBatteryData . EstimatedFlightTime < 120 ) {
2013-05-19 16:37:30 +02:00
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]*/
2016-11-04 00:27:18 +01:00
if ( flightBatteryData . Voltage < batterySettings . CellVoltageThresholds . Critical * flightBatteryData . NbCells ) {
2013-05-19 16:37:30 +02:00
AlarmsSet ( SYSTEMALARMS_ALARM_BATTERY , SYSTEMALARMS_ALARM_CRITICAL ) ;
2014-04-10 17:28:48 +02:00
} else if ( flightBatteryData . Voltage < batterySettings . CellVoltageThresholds . Warning * flightBatteryData . NbCells ) {
2013-05-19 16:37:30 +02:00
AlarmsSet ( SYSTEMALARMS_ALARM_BATTERY , SYSTEMALARMS_ALARM_WARNING ) ;
} else {
AlarmsClear ( SYSTEMALARMS_ALARM_BATTERY ) ;
}
}
FlightBatteryStateSet ( & flightBatteryData ) ;
2010-09-27 09:28:34 +02:00
}
2014-04-10 17:28:48 +02:00
2017-02-20 22:29:43 +01:00
static void GetNbCells ( const FlightBatterySettingsData * batterySettings , FlightBatteryStateData * flightBatteryData )
2014-04-10 17:28:48 +02:00
{
// get flight status to check for armed
2014-04-13 14:08:34 +02:00
uint8_t armed = 0 ;
2017-02-20 22:29:43 +01:00
static bool detected = false ;
2014-04-13 14:08:34 +02:00
2017-02-20 22:29:43 +01:00
// prevent the cell number to change once the board is armed at least once
if ( detected ) {
return ;
}
2014-04-13 14:08:34 +02:00
2017-02-20 22:29:43 +01:00
FlightStatusArmedGet ( & armed ) ;
2014-04-10 17:28:48 +02:00
// check only if not armed
2014-04-13 14:07:16 +02:00
if ( armed = = FLIGHTSTATUS_ARMED_ARMED ) {
2017-02-20 22:29:43 +01:00
detected = true ;
return ;
2014-04-10 17:28:48 +02:00
}
// prescribed number of cells?
if ( batterySettings - > NbCells ! = 0 ) {
flightBatteryData - > NbCells = batterySettings - > NbCells ;
flightBatteryData - > NbCellsAutodetected = 0 ;
2017-02-20 22:29:43 +01:00
return ;
2014-04-10 17:28:48 +02:00
}
// plausibility check
if ( flightBatteryData - > Voltage < = 0.5f ) {
// cannot detect number of cells
flightBatteryData - > NbCellsAutodetected = 0 ;
2017-02-20 22:29:43 +01:00
return ;
2014-04-10 17:28:48 +02:00
}
float voltageMin = 0.f , voltageMax = 0.f ;
// Cell type specific values
// TODO: could be implemented as constant arrays indexed by cellType
// or could be part of the UAVObject definition
switch ( batterySettings - > Type ) {
case FLIGHTBATTERYSETTINGS_TYPE_LIPO :
case FLIGHTBATTERYSETTINGS_TYPE_LICO :
voltageMin = 3.6f ;
voltageMax = 4.2f ;
break ;
2016-12-06 01:00:17 +01:00
case FLIGHTBATTERYSETTINGS_TYPE_LIHV :
voltageMin = 3.6f ;
voltageMax = 4.35f ;
break ;
2014-04-10 17:28:48 +02:00
case FLIGHTBATTERYSETTINGS_TYPE_A123 :
voltageMin = 2.01f ;
voltageMax = 3.59f ;
break ;
case FLIGHTBATTERYSETTINGS_TYPE_LIFESO4 :
default :
flightBatteryData - > NbCellsAutodetected = 0 ;
2017-02-20 22:29:43 +01:00
return ;
2014-04-10 17:28:48 +02:00
}
// uniquely measurable under any condition iff n * voltageMax < (n+1) * voltageMin
// or n < voltageMin / (voltageMax-voltageMin)
// weaken condition by setting n <= voltageMin / (voltageMax-voltageMin) and
// checking for v <= voltageMin * voltageMax / (voltageMax-voltageMin)
if ( flightBatteryData - > Voltage > voltageMin * voltageMax / ( voltageMax - voltageMin ) ) {
flightBatteryData - > NbCellsAutodetected = 0 ;
2017-02-20 22:29:43 +01:00
return ;
2014-04-10 17:28:48 +02:00
}
2017-02-20 22:29:43 +01:00
// Prevent the battery discharging on the ground to change the detected number of cells:
// Detection is enabled in the first 60 seconds from powerup
uint32_t flightTime ;
SystemStatsFlightTimeGet ( & flightTime ) ;
if ( flightTime > DETECTION_TIMEFRAME ) {
detected = true ;
}
2014-04-10 17:28:48 +02:00
flightBatteryData - > NbCells = ( int8_t ) ( flightBatteryData - > Voltage / voltageMin ) ;
flightBatteryData - > NbCellsAutodetected = 1 ;
}
2010-09-27 09:28:34 +02:00
/**
2013-05-19 16:37:30 +02:00
* @ }
*/
2010-09-27 09:28:34 +02:00
/**
* @ }
*/