static inline uint8_t pprz_mode_update( void )
{
if ((pprz_mode != PPRZ_MODE_HOME &&
pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER)
#ifdef UNLOCKED_HOME_MODE
|| TRUE
#endif
)
{
#ifndef RADIO_AUTO_MODE
return ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE]));
#else
INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
/* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between two switches/channels
* RADIO_MODE will switch between PPRZ_MODE_MANUAL and any PPRZ_MODE_AUTO mode selected by RADIO_AUTO_MODE.
*
* This is mainly a cludge for entry level radios with no three-way switch but two available two-way switches which can be used.
*这主要是一个入门低级无线电台的组装机，没有三项开关，但是可以用两相开关。
*/
if(PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE]) == PPRZ_MODE_MANUAL) {
/* RADIO_MODE in MANUAL position */
return ModeUpdate(pprz_mode, PPRZ_MODE_MANUAL);
}
else
{
/* RADIO_MODE not in MANUAL position.
* Select AUTO mode bassed on RADIO_AUTO_MODE channel
*/
return ModeUpdate(pprz_mode, (fbw_state->channels[RADIO_AUTO_MODE] > THRESHOLD2) ? PPRZ_MODE_AUTO2 : PPRZ_MODE_AUTO1);
}
#endif // RADIO_AUTO_MODE
}
else
return FALSE;
static inline uint8_t pprz_mode_update( void ) {
if ((pprz_mode != PPRZ_MODE_HOME &&
pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER)
#ifdef UNLOCKED_HOME_MODE
|| TRUE
#endif
) {
return ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE], fbw_state->status));
} else
return FALSE;
}
/********** EVENT ************************************************************/
void event_task_rctx( void) {
#ifdef RADIO_CONTROL
if (ppm_valid) {
ppm_valid = FALSE;
radio_control_event_task();
#ifdef USE_RCTX_MODE_SWITCH
// TODO: set rxtx_mode from GPIO connected switch (e.g. I2C pins)
#else
rctx_mode = PPRZ_MODE_OF_PULSE(rc_values[RADIO_MODE], 0) & 3;
#endif
rctx_mode |= rctx_under_voltage << 2;
LED_TOGGLE(3);
if (1)
// TODO: check XBee busy pin
// TODO: send only if aircraft is listening
// TODO: send (here) only in auto1 and manual
{
DOWNLINK_SEND_RC_3CH(
&rctx_mode,
&rc_values[RADIO_THROTTLE],
&rc_values[RADIO_ROLL],
&rc_values[RADIO_PITCH]);
}
}
#endif
#if defined DATALINK
#if DATALINK == XBEE
if (XBeeBuffer()) {
ReadXBeeBuffer();
if (xbee_msg_received) {
xbee_parse_payload();
xbee_msg_received = FALSE;
}
}
#endif
if (dl_msg_available) {
dl_parse_msg();
dl_msg_available = FALSE;
}
#endif /* DATALINK */
}
/** 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);
}
