void event_task_ap( void ) { /***** Datalink *******/ if (PprzBuffer()) { ReadPprzBuffer(); if (pprz_msg_received) { pprz_parse_payload(); pprz_msg_received = FALSE; if (dl_msg_available) { dl_parse_msg(); dl_msg_available = FALSE; } } } /* parse and use GPS messages */ if (GpsBuffer()) { ReadGpsBuffer(); } if (gps_msg_received) { parse_gps_msg(); gps_msg_received = FALSE; if (gps_pos_available) { use_gps_pos(); gps_pos_available = FALSE; } } }
int main( void ) { uint8_t init_cpt; /* init peripherals */ timer_init(); modem_init(); adc_init(); #ifdef CTL_BRD_V1_1 adc_buf_channel(ADC_CHANNEL_BAT, &buf_bat); #endif spi_init(); link_fbw_init(); gps_init(); nav_init(); ir_init(); estimator_init(); # ifdef PAPABENCH_SINGLE fbw_init(); # endif /* start interrupt task */ //sei(); /*Fadia*/ /* Wait 0.5s (for modem init ?) */ init_cpt = 30; _Pragma("loopbound min 31 max 31") while (init_cpt) { if (timer_periodic()) init_cpt--; } /* enter mainloop */ #ifndef NO_MAINLOOP while( 1 ) { #endif if(timer_periodic()) { periodic_task(); # if PAPABENCH_SINGLE fbw_schedule(); # endif } if (gps_msg_received) { /*receive_gps_data_task()*/ parse_gps_msg(); send_gps_pos(); send_radIR(); send_takeOff(); } if (link_fbw_receive_complete) { link_fbw_receive_complete = FALSE; radio_control_task(); } #ifndef NO_MAINLOOP } #endif return 0; }
/*********** EVENT ***********************************************************/ void event_task_ap( void ) { #ifdef USE_INFRARED infrared_event(); #endif #ifdef USE_AHRS ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event); #endif // USE_AHRS #ifdef USE_GPS #if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the datalink */ if (GpsBuffer()) { ReadGpsBuffer(); } #endif if (gps_msg_received) { /* parse and use GPS messages */ #ifdef GPS_CONFIGURE if (gps_configuring) gps_configure(); else #endif parse_gps_msg(); gps_msg_received = FALSE; if (gps_pos_available) { gps_verbose_downlink = !launch; UseGpsPosNoSend(estimator_update_state_gps); gps_downlink(); #ifdef GPS_TRIGGERED_FUNCTION #ifndef SITL GPS_TRIGGERED_FUNCTION(); #endif #endif gps_pos_available = FALSE; } } #endif /** USE_GPS */ DatalinkEvent(); #ifdef MCU_SPI_LINK link_mcu_event_task(); #endif if (inter_mcu_received_fbw) { /* receive radio control task from fbw */ inter_mcu_received_fbw = FALSE; telecommand_task(); } modules_event_task(); } /* event_task_ap() */
/** * @brief Receive communication packets and handle them * * This function decodes packets on the protocol level and also handles * their value by calling the appropriate functions. */ void communication_receive(void) { mavlink_message_t msg; mavlink_status_t status = { 0 }; status.packet_rx_drop_count = 0; // COMMUNICATION WITH ONBOARD COMPUTER while (uart0_char_available()) { uint8_t c = uart0_get_char(); if (global_data.state.uart0mode == UART_MODE_MAVLINK) { // Try to get a new message if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // Handle message handle_mavlink_message(MAVLINK_COMM_0, &msg); } } else if (global_data.state.uart0mode == UART_MODE_BYTE_FORWARD) { uart1_transmit(c); } // And get the next one } // Update global packet drops counter global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count; global_data.comm.uart0_rx_success_count += status.packet_rx_success_count; status.packet_rx_drop_count = 0; // COMMUNICATION WITH EXTERNAL COMPUTER while (uart1_char_available()) { uint8_t c = uart1_get_char(); // Check if this link is used for MAVLink or GPS if (global_data.state.uart1mode == UART_MODE_MAVLINK) { //uart0_transmit((unsigned char)c); // Try to get a new message if (mavlink_parse_char(MAVLINK_COMM_1, c, &msg, &status)) { // Handle message handle_mavlink_message(MAVLINK_COMM_1, &msg); } } else if (global_data.state.uart1mode == UART_MODE_GPS) { if (global_data.state.gps_mode == 10) { static uint8_t gps_i = 0; static char gps_chars[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; if (c == '$' || gps_i == MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN - 1) { gps_i = 0; char gps_chars_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; strncpy(gps_chars_buf, gps_chars, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); debug_message_buffer(gps_chars_buf); } gps_chars[gps_i++] = c; } if (gps_parse(c)) { // New GPS data received //debug_message_buffer("RECEIVED NEW GPS DATA"); parse_gps_msg(); if (gps_lat == 0) { global_data.state.gps_ok = 0; //debug_message_buffer("GPS Signal Lost"); } else { global_data.state.gps_ok = 1; mavlink_msg_gps_raw_send( global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_time(), gps_mode, gps_lat / 1e7f, gps_lon / 1e7f, gps_alt / 100.0f, 0.0f, 0.0f, gps_gspeed / 100.0f, gps_course / 10.0f); } // // Output satellite info // for (int i = 0; i < gps_nb_channels; i++) // { // mavlink_msg_gps_status_send(global_data.param[PARAM_SEND_DEBUGCHAN], gps_numSV, gps_svinfos[i].svid, gps_satellite_used(gps_svinfos[i].qi), gps_svinfos[i].elev, ((gps_svinfos[i].azim/360.0f)*255.0f), gps_svinfos[i].cno); // } } } else if (global_data.state.uart1mode == UART_MODE_BYTE_FORWARD) { uart0_transmit(c); led_toggle(LED_YELLOW); } // And get the next one } // Update global packet drops counter global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count; global_data.comm.uart0_rx_success_count += status.packet_rx_success_count; status.packet_rx_drop_count = 0; }
/*********** EVENT ***********************************************************/ void event_task_ap( void ) { #ifdef USE_ANALOG_IMU ImuEvent(on_gyro_accel_event, on_mag_event); #endif // USE_ANALOG_IMU #ifdef USE_GPS #if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the datalink */ if (GpsBuffer()) { ReadGpsBuffer(); } #endif if (gps_msg_received) { /* parse and use GPS messages */ #ifdef GPS_CONFIGURE if (gps_configuring) gps_configure(); else #endif parse_gps_msg(); gps_msg_received = FALSE; if (gps_pos_available) { gps_verbose_downlink = !launch; UseGpsPosNoSend(estimator_update_state_gps); gps_downlink(); #ifdef GPS_TRIGGERED_FUNCTION #ifndef SITL GPS_TRIGGERED_FUNCTION(); #endif #endif gps_pos_available = FALSE; } } #endif /** USE_GPS */ #if defined DATALINK #if DATALINK == PPRZ if (PprzBuffer()) { ReadPprzBuffer(); if (pprz_msg_received) { pprz_parse_payload(); pprz_msg_received = FALSE; } } #elif DATALINK == XBEE if (XBeeBuffer()) { ReadXBeeBuffer(); if (xbee_msg_received) { xbee_parse_payload(); xbee_msg_received = FALSE; } } #else #error "Unknown DATALINK" #endif if (dl_msg_available) { dl_parse_msg(); dl_msg_available = FALSE; } #endif /** DATALINK */ #ifdef MCU_SPI_LINK link_mcu_event_task(); #endif if (inter_mcu_received_fbw) { /* receive radio control task from fbw */ inter_mcu_received_fbw = FALSE; telecommand_task(); } modules_event_task(); } /* event_task_ap() */
void vTask_8() { parse_gps_msg(); use_gps_pos(); }