Esempio n. 1
0
STATIC_INLINE void main_event(void)
{
  /* event functions for mcu peripherals: i2c, usb_serial.. */
  mcu_event();

  DatalinkEvent();

  if (autopilot_rc) {
    RadioControlEvent(autopilot_on_rc_frame);
  }

#if USE_IMU
  ImuEvent();
#endif

#ifdef InsEvent
  TODO("calling InsEvent, remove me..")
  InsEvent();
#endif

#if USE_BARO_BOARD
  BaroEvent();
#endif

#if USE_GPS
  GpsEvent();
#endif

#if FAILSAFE_GROUND_DETECT || KILL_ON_GROUND_DETECT
  DetectGroundEvent();
#endif

  modules_event_task();
}
Esempio n. 2
0
/*********** EVENT ***********************************************************/
void event_task_ap(void)
{

#ifndef SINGLE_MCU
  /* for SINGLE_MCU done in main_fbw */
  /* event functions for mcu peripherals: i2c, usb_serial.. */
  mcu_event();
#endif /* SINGLE_MCU */

#if USE_BARO_BOARD
  BaroEvent();
#endif

  modules_event_task();

#if defined MCU_SPI_LINK || defined MCU_UART_LINK
  link_mcu_event_task();
#endif

  if (inter_mcu_received_fbw) {
    /* receive radio control task from fbw */
    inter_mcu_received_fbw = false;
    autopilot_on_rc_frame();
  }

  autopilot_event();

} /* event_task_ap() */
Esempio n. 3
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() */
Esempio n. 4
0
STATIC_INLINE void main_event(void)
{
  /* event functions for mcu peripherals: i2c, usb_serial.. */
  mcu_event();

  // Handle RC
  RadioControlEvent(autopilot_on_rc_frame);

  // InterMCU
  InterMcuEvent(autopilot_on_ap_command);

  //Modules
  modules_event_task();
}
Esempio n. 5
0
STATIC_INLINE void main_event(void)
{
  /* Event functions for mcu peripherals: i2c, usb_serial.. */
  mcu_event();

  /* Handle RC */
  RadioControlEvent(fbw_on_rc_frame);

  /* InterMCU (gives autopilot commands as output) */
  InterMcuEvent(fbw_on_ap_command);

  /* FBW modules */
  modules_event_task();
}
Esempio n. 6
0
/*********** EVENT ***********************************************************/
void event_task_ap( void ) {

#ifndef SINGLE_MCU
#if defined USE_I2C0  || defined USE_I2C1  || defined USE_I2C2
  i2c_event();
#endif
#endif

#if USE_AHRS && USE_IMU
  ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif

#if USE_INS
  InsEvent(NULL);
#endif

#if USE_GPS
  GpsEvent(on_gps_solution);
#endif /* USE_GPS */

#if USE_BAROMETER
  BaroEvent(on_baro_abs_event, on_baro_dif_event);
#endif

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

#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
  if (new_ins_attitude > 0)
  {
    attitude_loop();
    //LED_TOGGLE(3);
    new_ins_attitude = 0;
  }
#endif

} /* event_task_ap() */
Esempio n. 7
0
static inline void main_event_task(void)
{
  modules_event_task();

  // spi baro
  if (spi_message_received) {
    /* Got a message on SPI. */
    spi_message_received = false;
    wt_baro_event();
    uint16_t temp = 0;
    float alt = 0.;
    DOWNLINK_SEND_BARO_MS5534A(&wt_baro_pressure, &temp, &alt);
  }

}
Esempio n. 8
0
void main_event(void)
{
  /* event functions for mcu peripherals: i2c, usb_serial.. */
  mcu_event();

  if (autopilot.use_rc) {
    RadioControlEvent(autopilot_on_rc_frame);
  }

#if USE_BARO_BOARD
  BaroEvent();
#endif

  autopilot_event();

  modules_event_task();
}
Esempio n. 9
0
STATIC_INLINE void main_event(void)
{
    /* event functions for mcu peripherals: i2c, usb_serial.. */
    mcu_event();

    if (autopilot_rc) {
        RadioControlEvent(autopilot_on_rc_frame);
    }

#if USE_BARO_BOARD
    BaroEvent();
#endif

#if FAILSAFE_GROUND_DETECT || KILL_ON_GROUND_DETECT
    DetectGroundEvent();
#endif

    modules_event_task();
}
Esempio n. 10
0
/*********** EVENT ***********************************************************/
void event_task_ap( void ) {

#ifdef USE_INFRARED
  infrared_event();
#endif

#ifdef USE_AHRS
  ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif // USE_AHRS

#ifdef USE_GPS
  GpsEvent(on_gps_solution);
#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();

#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
  if (new_ins_attitude > 0)
  {
    attitude_loop();
    //LED_TOGGLE(3);
    new_ins_attitude = 0;
  }
#endif

} /* event_task_ap() */
Esempio n. 11
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() */