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
}
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(DefaultChannel, DefaultDevice, &aoa_adc.raw, &aoa_adc.angle));
}
