static void onTimer(UAVObjEvent* ev) { static portTickType lastSysTime; static bool firstRun = true; static FlightBatteryStateData flightBatteryData; if (firstRun) { #ifdef ENABLE_DEBUG_MSG PIOS_COM_ChangeBaud(DEBUG_PORT, 57600); #endif lastSysTime = xTaskGetTickCount(); //FlightBatteryStateGet(&flightBatteryData); firstRun = false; } AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); portTickType thisSysTime; FlightBatterySettingsData batterySettings; static float dT = SAMPLE_PERIOD_MS / 1000; float Bob; float energyRemaining; // Check how long since last update thisSysTime = xTaskGetTickCount(); if(thisSysTime > lastSysTime) // reuse dt in case of wraparound dT = (float)(thisSysTime - lastSysTime) / (float)(portTICK_RATE_MS * 1000.0f); //lastSysTime = thisSysTime; FlightBatterySettingsGet(&batterySettings); //calculate the battery parameters flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(2)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts flightBatteryData.Current = ((float)PIOS_ADC_PinGet(1)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in Amps Bob =dT; // FIXME: something funky happens if I don't do this... Andrew flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * 1000.0 * dT / 3600.0) ;//in mAh if (flightBatteryData.Current > flightBatteryData.PeakCurrent)flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps flightBatteryData.AvgCurrent=(flightBatteryData.AvgCurrent*0.8)+(flightBatteryData.Current*0.2); //in Amps //sanity checks if (flightBatteryData.AvgCurrent<0)flightBatteryData.AvgCurrent=0.0; if (flightBatteryData.PeakCurrent<0)flightBatteryData.PeakCurrent=0.0; if (flightBatteryData.ConsumedEnergy<0)flightBatteryData.ConsumedEnergy=0.0; energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh flightBatteryData.EstimatedFlightTime = ((energyRemaining / (flightBatteryData.AvgCurrent*1000.0))*3600.0);//in Sec //generate alarms where needed... if ((flightBatteryData.Voltage<=0)&&(flightBatteryData.Current<=0)) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR); } else { if (flightBatteryData.EstimatedFlightTime < 30) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.EstimatedFlightTime < 60) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME); // FIXME: should make the battery voltage detection dependent on battery type. if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); } lastSysTime = thisSysTime; FlightBatteryStateSet(&flightBatteryData); }
static void onTimer(UAVObjEvent* ev) { static FlightBatteryStateData flightBatteryData; FlightBatterySettingsData batterySettings; FlightBatterySettingsGet(&batterySettings); static float dT = SAMPLE_PERIOD_MS / 1000.0f; float energyRemaining; //calculate the battery parameters if (voltageADCPin >=0) { flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(voltageADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts } else { flightBatteryData.Voltage=1234; //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 = ((float)PIOS_ADC_PinGet(currentADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_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.1234f; //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.*/ // //sanity checks // if (flightBatteryData.AvgCurrent<0) flightBatteryData.AvgCurrent=0.0f; // if (flightBatteryData.PeakCurrent<0) flightBatteryData.PeakCurrent=0.0f; // if (flightBatteryData.ConsumedEnergy<0) flightBatteryData.ConsumedEnergy=0.0f; energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh if (flightBatteryData.AvgCurrent > 0) flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent*1000.0f))*3600.0f;//in Sec else flightBatteryData.EstimatedFlightTime = 9999; //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 (flightBatteryData.EstimatedFlightTime < 30) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); else if (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.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); } FlightBatteryStateSet(&flightBatteryData); }
/** * Main task. It does not return. */ static void batteryTask(void * parameters) { const float dT = SAMPLE_PERIOD_MS / 1000.0f; settingsUpdatedCb(NULL); // Main task loop portTickType lastSysTime; lastSysTime = xTaskGetTickCount(); while (true) { vTaskDelayUntil(&lastSysTime, MS2TICKS(SAMPLE_PERIOD_MS)); FlightBatteryStateData flightBatteryData; FlightBatterySettingsData batterySettings; float energyRemaining; if (battery_settings_updated) { battery_settings_updated = false; FlightBatterySettingsGet(&batterySettings); voltageADCPin = batterySettings.VoltagePin; if (voltageADCPin == FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE) voltageADCPin = -1; currentADCPin = batterySettings.CurrentPin; if (currentADCPin == FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE) currentADCPin = -1; } //calculate the battery parameters if (voltageADCPin >= 0) { flightBatteryData.Voltage = ((float) PIOS_ADC_GetChannelVolt(voltageADCPin)) / batterySettings.SensorCalibrationFactor[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONFACTOR_VOLTAGE] * 1000.0f + batterySettings.SensorCalibrationOffset[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONOFFSET_VOLTAGE]; //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 = ((float) PIOS_ADC_GetChannelVolt(currentADCPin)) / batterySettings.SensorCalibrationFactor[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONFACTOR_CURRENT] * 1000.0f + batterySettings.SensorCalibrationOffset[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONOFFSET_CURRENT]; //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 = -1; //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 energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh if (flightBatteryData.AvgCurrent > 0) flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent * 1000.0f)) * 3600.0f; //in Sec else flightBatteryData.EstimatedFlightTime = 9999; //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 (flightBatteryData.EstimatedFlightTime < 30) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); else if (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.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); } FlightBatteryStateSet(&flightBatteryData); } }