static void csc_main_periodic( void )
{
PeriodicSendAp(DefaultChannel);
radio_control_periodic_task();
cpu_time++;
}
/** \brief Send a serie of initialisation messages followed by a stream of periodic ones
*
* Called at 60Hz.
*/
static inline void reporting_task( void ) {
static uint8_t boot = TRUE;
/** initialisation phase during boot */
if (boot) {
DOWNLINK_SEND_BOOT(DefaultChannel, &version);
boot = FALSE;
}
/** then report periodicly */
else {
PeriodicSendAp(DefaultChannel);
}
}
/** 60Hz */
void periodic_task_ap( void ) {
PORTD |= 1 << 5;
static uint8_t _10Hz = 0;
_10Hz++;
if (_10Hz>=6) _10Hz = 0;
if (!_10Hz) {
PeriodicSendAp();
if (sum_err_reset) {
sum_err_reset = FALSE;
ctl_grz_reset();
}
}
static uint16_t last_rangemeters[NB_RANGES];
static uint8_t last_idx;
static uint16_t sum_rangemeters = 0;
// ClearBit(ADCSRA, ADIE);
rangemeter = RangeCmOfAdc(rangemeter_adc_buf.sum/rangemeter_adc_buf.av_nb_sample);
// SetBit(ADCSRA, ADIE);
last_idx++;
if (last_idx >= NB_RANGES) last_idx = 0;
float last_avg = (float)sum_rangemeters / NB_RANGES;
sum_rangemeters += rangemeter - last_rangemeters[last_idx];
last_rangemeters[last_idx] = rangemeter;
float avg = (float)sum_rangemeters / NB_RANGES;
ctl_grz_z = avg / 100.; /* cm -> m */
ctl_grz_z_dot = ((avg - last_avg) * 61. / 100.);
/** Flying ? */
static uint8_t flying_cpt;
if (!flying) {
if (rangemeter > FLYING_MIN_CM) {
flying_cpt++;
if (flying_cpt > SWITCH_PEDIOD_MIN) {
flying = TRUE;
flying_cpt = 0;
}
} else
flying_cpt = 0;
} else if (rangemeter < STILL_MAX_CM) {
flying_cpt++;
if (flying_cpt > SWITCH_PEDIOD_MIN) {
flying = FALSE;
flying_cpt = 0;
}
} else
flying_cpt = 0;
pprz_mode = PPRZ_MODE_OF_PULSE(rc_values[RADIO_MODE], FIXME);
if (pprz_mode == PPRZ_MODE_AUTO2) {
ctl_grz_set_setpoints_auto2();
} else if (pprz_mode == PPRZ_MODE_AUTO1) {
ctl_grz_set_setpoints_auto1();
}
if (pprz_mode >= PPRZ_MODE_AUTO2) {
ctl_grz_z_dot_run();
/* if (!_10Hz) */
/* ctl_grz_horiz_speed_run(); */
}
if (pprz_mode >= PPRZ_MODE_AUTO1) {
// ctl_grz_alt_run();
// ctl_grz_z_dot_run();
ctl_grz_attitude_run();
}
PORTD &= ~ (1 << 5);
}