コード例 #1
0
ファイル: aoa_pwm.c プロジェクト: Merafour/paparazzi
void aoa_pwm_update(void) {
  static float prev_aoa = 0.0f;

  // raw duty cycle in usec
  uint32_t duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL);

  // remove some offset if needed
  aoa_pwm.raw = duty_raw - AOA_PWM_OFFSET;
  // FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097
  // this case is not handled since we don't care about angles close to +- 180 deg
  aoa_pwm.angle = AOA_SIGN * (((float)aoa_pwm.raw * aoa_pwm.sens) - aoa_pwm.offset - AOA_ANGLE_OFFSET);
  // filter angle
  aoa_pwm.angle = aoa_pwm.filter * prev_aoa + (1.0f - aoa_pwm.filter) * aoa_pwm.angle;
  prev_aoa = aoa_pwm.angle;

#if USE_AOA
  stateSetAngleOfAttack_f(aoa_adc.angle);
#endif

#if SEND_SYNC_AOA
  RunOnceEvery(10, DOWNLINK_SEND_AOA(DefaultChannel, DefaultDevice, &aoa_pwm.raw, &aoa_pwm.angle));
#endif

#if LOG_AOA
  if(pprzLogFile != -1) {
    if (!log_started) {
      sdLogWriteLog(pprzLogFile, "AOA_PWM: ANGLE(deg) RAW(int16)\n");
      log_started = true;
    } else {
      float angle = DegOfRad(aoa_pwm.angle);
      sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d\n", angle, aoa_pwm.raw);
    }
  }
#endif
}
コード例 #2
0
ファイル: AOA_adc.c プロジェクト: rdeandrade/paparazzi
void AOA_adc_update( void ) {
#ifndef SITL
    adc_AOA_val = buf_AOA.sum / buf_AOA.av_nb_sample;

// 	PT1 filter and convert to rad
    AOA = AOA_filter * AOA_old + (1 - AOA_filter) * (adc_AOA_val*(2*M_PI)/1024-M_PI+AOA_offset);
    AOA_old = AOA;
#endif
    RunOnceEvery(30, DOWNLINK_SEND_AOA_adc(DefaultChannel, DefaultDevice, &adc_AOA_val, &AOA));

#ifdef USE_AOA
    stateSetAngleOfAttack_f(AOA);
#endif
}
コード例 #3
0
ファイル: aoa_adc.c プロジェクト: 1bitsquared/paparazzi
void aoa_adc_update(void) {
  static float prev_aoa = 0.0;

  aoa_adc.raw = aoa_adc.buf.sum / aoa_adc.buf.av_nb_sample;

  // PT1 filter and convert to rad
  aoa_adc.angle = aoa_adc.filter * prev_aoa +
    (1.0 - aoa_adc.filter) * (aoa_adc.raw * aoa_adc.sens - aoa_adc.offset);
  prev_aoa = aoa_adc.angle;

#ifdef USE_AOA
  stateSetAngleOfAttack_f(&aoa_adc.angle);
#endif

  RunOnceEvery(30, DOWNLINK_SEND_AOA_ADC(DefaultChannel, DefaultDevice, &aoa_adc.raw, &aoa_adc.angle));
}