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