/*********** 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() */
value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m, value lat, value lon) { gps_mode = (Bool_val(m) ? 3 : 0); gps_course = DegOfRad(Double_val(c)) * 10.; gps_alt = Double_val(a) * 100.; gps_gspeed = Double_val(s) * 100.; gps_climb = Double_val(cl) * 100.; gps_week = 0; // FIXME gps_itow = Double_val(t) * 1000.; #ifdef GPS_USE_LATLONG gps_lat = DegOfRad(Double_val(lat))*1e7; gps_lon = DegOfRad(Double_val(lon))*1e7; latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0); gps_utm_east = latlong_utm_x * 100; gps_utm_north = latlong_utm_y * 100; gps_utm_zone = nav_utm_zone0; x = y = z; /* Just to get rid of the "unused arg" warning */ #else // GPS_USE_LATLONG gps_utm_east = Int_val(x); gps_utm_north = Int_val(y); gps_utm_zone = Int_val(z); lat = lon; /* Just to get rid of the "unused arg" warning */ #endif // GPS_USE_LATLONG /** Space vehicle info simulation */ gps_nb_channels=7; int i; static int time; time++; for(i = 0; i < gps_nb_channels; i++) { gps_svinfos[i].svid = 7 + i; gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360; gps_svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; gps_svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8; } gps_PDOP = gps_Sacc = gps_Pacc = 500+200*sin(time/100.); gps_numSV = 7; gps_verbose_downlink = !launch; UseGpsPosNoSend(estimator_update_state_gps); gps_downlink(); return Val_unit; }
/*********** 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() */