/*********** 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() */
