void electrical_periodic(void) {
#ifndef SITL
electrical.adc_battery = electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample;
electrical.vsupply = 10*(float)VoltageOfAdc((float)electrical.adc_battery);
#endif
#ifdef ADC_CHANNEL_CURRENT
#ifndef SITL
electrical.current = MilliAmpereOfAdc((electrical_priv.current_adc_buf.sum/electrical_priv.current_adc_buf.av_nb_sample));
/* Prevent an overflow on high current spikes when using the motor brake */
BoundAbs(electrical.current, 65000);
#endif
#else
#if defined MILLIAMP_AT_FULL_THROTTLE && defined COMMAND_THROTTLE
/*
* Superellipse: abs(x/a)^n + abs(y/b)^n = 1
* with a = 1
* b = mA at full throttle
* n = 1.2 This defines nonlinearity (1 = linear)
* x = throttle
* y = current
*
* define CURRENT_ESTIMATION_NONLINEARITY in your airframe file to change the default nonlinearity factor of 1.2
*/
float b = (float)MILLIAMP_AT_FULL_THROTTLE;
float x = ((float)commands[COMMAND_THROTTLE]) / ((float)MAX_PPRZ);
/* electrical.current y = ( b^n - (b* x/a)^n )^1/n
* a=1, n = electrical_priv.nonlin_factor
*/
electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor));
#endif
#endif /* ADC_CHANNEL_CURRENT */
}
void electrical_periodic(void) {
static uint8_t bat_low_counter = 0;
static uint8_t bat_critical_counter = 0;
#if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL)
electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample));
#endif
#ifdef ADC_CHANNEL_CURRENT
#ifndef SITL
electrical.current = MilliAmpereOfAdc((electrical_priv.current_adc_buf.sum/electrical_priv.current_adc_buf.av_nb_sample));
/* Prevent an overflow on high current spikes when using the motor brake */
BoundAbs(electrical.current, 65000);
#endif
#elif defined MILLIAMP_AT_FULL_THROTTLE && defined COMMAND_CURRENT_ESTIMATION
/*
* Superellipse: abs(x/a)^n + abs(y/b)^n = 1
* with a = 1
* b = mA at full throttle
* n = 1.2 This defines nonlinearity (1 = linear)
* x = throttle
* y = current
*
* define CURRENT_ESTIMATION_NONLINEARITY in your airframe file to change the default nonlinearity factor of 1.2
*/
float b = (float)MILLIAMP_AT_FULL_THROTTLE;
float x = ((float)commands[COMMAND_CURRENT_ESTIMATION]) / ((float)MAX_PPRZ);
/* electrical.current y = ( b^n - (b* x/a)^n )^1/n
* a=1, n = electrical_priv.nonlin_factor
*/
electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor));
#endif /* ADC_CHANNEL_CURRENT */
if (electrical.vsupply < LOW_BAT_LEVEL * 10) {
if (bat_low_counter > 0)
bat_low_counter--;
if (bat_low_counter == 0)
electrical.bat_low = TRUE;
}
else {
// reset battery low status and counter
bat_low_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
electrical.bat_low = FALSE;
}
if (electrical.vsupply < CRITIC_BAT_LEVEL * 10) {
if (bat_critical_counter > 0)
bat_critical_counter--;
if (bat_critical_counter == 0)
electrical.bat_critical = TRUE;
}
else {
// reset battery critical status and counter
bat_critical_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
electrical.bat_critical = FALSE;
}
}
void electrical_periodic(void)
{
static uint32_t bat_low_counter = 0;
static uint32_t bat_critical_counter = 0;
static bool vsupply_check_started = false;
#if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL)
electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum /
electrical_priv.vsupply_adc_buf.av_nb_sample));
#endif
#ifdef ADC_CHANNEL_CURRENT
#ifndef SITL
int32_t current_adc = electrical_priv.current_adc_buf.sum / electrical_priv.current_adc_buf.av_nb_sample;
electrical.current = MilliAmpereOfAdc(current_adc);
/* Prevent an overflow on high current spikes when using the motor brake */
BoundAbs(electrical.current, 65000);
#endif
#elif defined MILLIAMP_AT_FULL_THROTTLE && defined COMMAND_CURRENT_ESTIMATION
/*
* Superellipse: abs(x/a)^n + abs(y/b)^n = 1
* with a = 1
* b = mA at full throttle
* n = 1.2 This defines nonlinearity (1 = linear)
* x = throttle
* y = current
*
* define CURRENT_ESTIMATION_NONLINEARITY in your airframe file to change the default nonlinearity factor of 1.2
*/
float full_current = (float)MILLIAMP_AT_FULL_THROTTLE;
float idle_current = (float)MILLIAMP_AT_IDLE_THROTTLE;
float x = ((float)commands[COMMAND_CURRENT_ESTIMATION]) / ((float)MAX_PPRZ);
/* Boundary check for x to prevent math errors due to negative numbers in
* pow() */
if(x > 1.0f) {
x = 1.0f;
} else if(x < 0.0f) {
x = 0.0f;
}
/* electrical.current y = ( b^n - (b* x/a)^n )^1/n
* a=1, n = electrical_priv.nonlin_factor
*/
#ifndef FBW
if(kill_throttle) {
// Assume no current when throttle killed (motors off)
electrical.current = 0;
} else {
#endif
electrical.current = full_current -
pow((pow(full_current - idle_current, electrical_priv.nonlin_factor) -
pow(((full_current - idle_current) * x), electrical_priv.nonlin_factor)),
(1. / electrical_priv.nonlin_factor));
#ifndef FBW
}
#endif
#endif /* ADC_CHANNEL_CURRENT */
// mAh = mA * dt (10Hz -> hours)
electrical.energy += ((float)electrical.current) / 3600.0f / ELECTRICAL_PERIODIC_FREQ;
/*if valid voltage is seen then start checking. Set min level to 0 to always start*/
if (electrical.vsupply >= MIN_BAT_LEVEL * 10) {
vsupply_check_started = true;
}
if (vsupply_check_started) {
if (electrical.vsupply < LOW_BAT_LEVEL * 10) {
if (bat_low_counter > 0) {
bat_low_counter--;
}
if (bat_low_counter == 0) {
electrical.bat_low = true;
}
} else {
// reset battery low status and counter
bat_low_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
electrical.bat_low = false;
}
if (electrical.vsupply < CRITIC_BAT_LEVEL * 10) {
if (bat_critical_counter > 0) {
bat_critical_counter--;
}
if (bat_critical_counter == 0) {
electrical.bat_critical = true;
}
} else {
// reset battery critical status and counter
bat_critical_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
electrical.bat_critical = false;
}
}
}
