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;
}
}
/************* PERIODIC ******************************************************/
void periodic_task_rctx( void ) {
static uint8_t _10Hz = 0;
static uint8_t _1Hz = 0;
_10Hz++;
if (_10Hz >= 6) _10Hz = 0;
_1Hz++;
if (_1Hz>=60) _1Hz=0;
#ifdef RADIO_CONTROL
radio_control_periodic_task();
#endif
#ifdef DOWNLINK
// TODO: needed? fbw_downlink_periodic_task();
#endif
#ifdef ADC
if (!_10Hz)
rctx_vsupply_decivolt = VoltageOfAdc((10*(vsupply_adc_buf.sum/vsupply_adc_buf.av_nb_sample)));
if (!_1Hz) {
static uint8_t t = 0;
if (rctx_vsupply_decivolt < LOW_BATTERY_DECIVOLT) {
t++;
} else {
t = 0;
rctx_under_voltage = 0;
}
if (t >= LOW_BATTERY_DELAY) {
LED_TOGGLE(1);
rctx_under_voltage = 1;
}
if (0)
// TODO: send (here) only in auto2
{
DOWNLINK_SEND_RC_3CH(
&rctx_mode,
&rc_values[RADIO_THROTTLE],
&rc_values[RADIO_ROLL],
&rc_values[RADIO_PITCH]);
}
}
#endif
}
/* Prepare data to be sent to mcu0 */
static void to_autopilot_from_last_radio (void) {
uint8_t i;
for(i = 0; i < RADIO_CTL_NB; i++)
to_mega128.channels[i] = last_radio[i];
to_mega128.status = (radio_ok ? _BV(STATUS_RADIO_OK) : 0);
to_mega128.status |= (radio_really_lost ? _BV(RADIO_REALLY_LOST) : 0);
if (last_radio_contains_avg_channels) {
to_mega128.status |= _BV(AVERAGED_CHANNELS_SENT);
last_radio_contains_avg_channels = FALSE;
}
to_mega128.ppm_cpt = last_ppm_cpt;
#ifndef CTL_BRD_V1_1
to_mega128.vsupply = VoltageOfAdc(vsupply_adc_buf.sum/AV_NB_SAMPLE) * 10;
#else
to_mega128.vsupply = 0;
#endif
}
/************* PERIODIC ******************************************************/
void periodic_task_fbw( void ) {
static uint8_t _10Hz; /* FIXME : sys_time should provide it */
_10Hz++;
if (_10Hz >= 6) _10Hz = 0;
#ifdef RADIO_CONTROL
radio_control_periodic_task();
if (fbw_mode == FBW_MODE_MANUAL && rc_status == RC_REALLY_LOST) {
fbw_mode = FBW_MODE_AUTO;
}
#endif
#ifdef INTER_MCU
inter_mcu_periodic_task();
if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
set_failsafe_mode();
#endif
#ifdef DOWNLINK
fbw_downlink_periodic_task();
#endif
#ifdef ADC
if (!_10Hz)
fbw_vsupply_decivolt = VoltageOfAdc((10*(vsupply_adc_buf.sum/vsupply_adc_buf.av_nb_sample)));
#endif
#ifdef ACTUATORS
#ifdef CTL_GRZ
if (rc_status == RC_REALLY_LOST) {
set_failsafe_mode();
} else {
ctl_grz_rate_run();
}
#endif /* CTL_GRZ */
SetActuatorsFromCommands(commands);
#endif
}
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;
}
}
}
