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; } } }