Exemplos de estimator_propagate_state em C++ (Cpp)

Exemplo n.º 1

Arquivo: main_ap.c Projeto: lxl/paparazzi

void periodic_task_ap( void ) {

  static uint8_t _60Hz = 0;
  static uint8_t _20Hz = 0;
  static uint8_t _10Hz = 0;
  static uint8_t _4Hz  = 0;
  static uint8_t _1Hz  = 0;

#ifdef USE_IMU
  // Run at PERIODIC_FREQUENCY (60Hz if not defined)
  imu_periodic();

#endif // USE_IMU

#define _check_periodic_freq_ PERIODIC_FREQUENCY % 60
#if _check_periodic_freq_
#error Using HighSpeed Periodic: PERIODIC_FREQUENCY has to be a multiple of 60!
#endif
  _60Hz++;
  if (_60Hz >= (PERIODIC_FREQUENCY / 60))
  {
    _60Hz = 0;
  }
  else
  {
    return;
  }


  // Rest of the periodic function still runs at 60Hz like always
  _20Hz++;
  if (_20Hz>=3) _20Hz=0;
  _10Hz++;
  if (_10Hz>=6) _10Hz=0;
  _4Hz++;
  if (_4Hz>=15) _4Hz=0;
  _1Hz++;
  if (_1Hz>=60) _1Hz=0;

  reporting_task();

  if (!_1Hz) {
    if (estimator_flight_time) estimator_flight_time++;
#if defined DATALINK || defined SITL
    datalink_time++;
#endif

    static uint8_t t = 0;
    if (vsupply < LOW_BATTERY_DECIVOLT) t++; else t = 0;
    kill_throttle |= (t >= LOW_BATTERY_DELAY);
    kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
  }

  switch(_4Hz) {
  case 0:
#ifdef SITL
#ifdef GPS_TRIGGERED_FUNCTION
    GPS_TRIGGERED_FUNCTION();
#endif
#endif
    estimator_propagate_state();
#ifdef EXTRA_DOWNLINK_DEVICE
    DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta);
#endif
    navigation_task();
    break;
  case 1:
    if (!estimator_flight_time &&
    estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) {
      estimator_flight_time = 1;
      launch = TRUE; /* Not set in non auto launch */
      DOWNLINK_SEND_TAKEOFF(DefaultChannel, &cpu_time_sec);
  default:
    break;
    }

    break;

#ifdef USE_GPIO
  case 3:
    GpioUpdate1();
    break;
#endif

    /*  default: */
  }

#ifndef CONTROL_RATE
#define CONTROL_RATE 20
#endif

#if CONTROL_RATE != 60 && CONTROL_RATE != 20
#error "Only 20 and 60 allowed for CONTROL_RATE"
#endif

#if CONTROL_RATE == 20
  if (!_20Hz)
#endif
    {

#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
      attitude_loop();
#endif

    }


  modules_periodic_task();
}

Exemplo n.º 2

Arquivo: main_ap.c Projeto: agreee45/SNUT

void periodic_task_ap( void ) {
  static uint8_t _20Hz   = 0;
  static uint8_t _10Hz   = 0;
  static uint8_t _4Hz   = 0;
  static uint8_t _1Hz   = 0;

  _20Hz++;
  if (_20Hz>=3) _20Hz=0;
  _10Hz++;
  if (_10Hz>=6) _10Hz=0;
  _4Hz++;
  if (_4Hz>=15) _4Hz=0;
  _1Hz++;
  if (_1Hz>=60) _1Hz=0;

  reporting_task();

  if (!_1Hz) {
    if (estimator_flight_time) estimator_flight_time++;
#if defined DATALINK || defined SITL
    datalink_time++;
#endif

    static uint8_t t = 0;
    if (vsupply < LOW_BATTERY_DECIVOLT) t++; else t = 0;
    kill_throttle |= (t >= LOW_BATTERY_DELAY);
    kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
  }

  switch(_4Hz) {
  case 0:
#ifdef SITL
#ifdef GPS_TRIGGERED_FUNCTION
    GPS_TRIGGERED_FUNCTION();
#endif
#endif
    estimator_propagate_state();
#ifdef EXTRA_DOWNLINK_DEVICE
    DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta);
#endif
    navigation_task();
    break;
  case 1:
    if (!estimator_flight_time &&
    estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) {
      estimator_flight_time = 1;
      launch = TRUE; /* Not set in non auto launch */
      DOWNLINK_SEND_TAKEOFF(DefaultChannel, &cpu_time_sec);
  default:
    break;
    }

    break;

#ifdef USE_GPIO
  case 3:
    GpioUpdate1();
    break;
#endif

    /*  default: */
  }

#ifndef CONTROL_RATE
#define CONTROL_RATE 20
#endif

#if CONTROL_RATE != 60 && CONTROL_RATE != 20
#error "Only 20 and 60 allowed for CONTROL_RATE"
#endif

#ifdef USE_ANALOG_IMU
  if (!_20Hz) {
    imu_periodic();
  }
#endif // USE_ANALOG_IMU

#if CONTROL_RATE == 20
  if (!_20Hz)
#endif
    {

#ifdef USE_GYRO
      gyro_update();
#endif

#ifdef USE_INFRARED
      infrared_update();
      estimator_update_state_infrared();
#endif /* USE_INFRARED */
      h_ctl_attitude_loop(); /* Set  h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
      v_ctl_throttle_slew();
      ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
      ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
      ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;

#if defined MCU_SPI_LINK
      link_mcu_send();
#elif defined INTER_MCU && defined SINGLE_MCU
      /**Directly set the flag indicating to FBW that shared buffer is available*/
      inter_mcu_received_ap = TRUE;
#endif

    }

  modules_periodic_task();
}