コード例 #1
0
void nps_autopilot_run_step(double time)
{

  nps_electrical_run_step(time);

#if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK
  if (nps_radio_control_available(time)) {
    radio_control_feed();
    main_event();
  }
#endif

  if (nps_sensors_gyro_available()) {
    imu_feed_gyro_accel();
    main_event();
  }

  if (nps_sensors_mag_available()) {
    imu_feed_mag();
    main_event();
  }

  if (nps_sensors_baro_available()) {
    float pressure = (float) sensors.baro.value;
    AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, pressure);
    main_event();
  }

#if USE_SONAR
  if (nps_sensors_sonar_available()) {
    float dist = (float) sensors.sonar.value;
    AbiSendMsgAGL(AGL_SONAR_NPS_ID, dist);

    uint16_t foo = 0;
    DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);

    main_event();
  }
#endif

  if (nps_sensors_gps_available()) {
    gps_feed_value();
    main_event();
  }

  if (nps_bypass_ahrs) {
    sim_overwrite_ahrs();
  }

  if (nps_bypass_ins) {
    sim_overwrite_ins();
  }

  handle_periodic_tasks();

  /* scale final motor commands to 0-1 for feeding the fdm */
  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
    autopilot.commands[i] = (double)motor_mixing.commands[i] / MAX_PPRZ;
  }
}
コード例 #2
0
ファイル: sonar_adc.c プロジェクト: Aishwaryabr/paparazzi
/** Read ADC value to update sonar measurement
 */
void sonar_adc_read(void) {
#ifndef SITL
  sonar_meas = sonar_adc.sum / sonar_adc.av_nb_sample;
  sonar_data_available = TRUE;
  sonar_distance = ((float)sonar_meas * sonar_scale) + sonar_offset;

#else // SITL
  sonar_distance = stateGetPositionEnu_f()->z;
  Bound(sonar_distance, 0.1f, 7.0f);
#endif // SITL

#ifdef SENSOR_SYNC_SEND_SONAR
  DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_meas, &sonar_distance);
#endif
}
コード例 #3
0
ファイル: sonar_adc.c プロジェクト: AE4317group07/paparazzi
/** Read ADC value to update sonar measurement
 */
void sonar_adc_read(void)
{
#ifndef SITL
  sonar_adc.meas = sonar_adc_buf.sum / sonar_adc_buf.av_nb_sample;
  sonar_adc.distance = (float)(sonar_adc.meas - sonar_adc.offset) * SONAR_SCALE;
#else // SITL
  sonar_adc.distance = stateGetPositionEnu_f()->z;
  Bound(sonar_adc.distance, 0.1f, 7.0f);
#endif // SITL

  // Send ABI message
  AbiSendMsgAGL(AGL_SONAR_ADC_ID, sonar_adc.distance);

#ifdef SENSOR_SYNC_SEND_SONAR
  // Send Telemetry report
  DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_adc.meas, &sonar_adc.distance);
#endif
}