void SONAR_MAXBOTIX12_IRQ_HANDLER(void)
{
if (TIM_GetITStatus(SONAR_MAXBOTIX12_TIM, TIM_IT_CC1) == SET)
{
TIM_ClearITPendingBit(SONAR_MAXBOTIX12_TIM, TIM_IT_CC1);
int32_t sonar_meas = TIM_GetCapture1(SONAR_MAXBOTIX12_TIM);
/* pulse_cnts to actual pulse width in us:
* pulse width = pulse_cnts * (prescaler+1)/(actual clock)
* with 58us per centimeter, the alt in cm is:
*/
//int32_t alt_mm = pulse_cnts * 10 * (69/72) / 58;
//sonar_alt
//if ((sonar_meas > (sonar_filter_val + 1500) || sonar_meas < (sonar_filter_val - 1500))
if ((sonar_meas > (sonar_filter_val + 3000) || sonar_meas < (sonar_filter_val - 3000))
&& (sonar_spike_cnt < 5)) {
sonar_spike_cnt++;
sonar_meas = sonar_filter_val;
}
else {
sonar_spike_cnt = 0;
}
//@TODO: check for angle > 10 deg ==> don't use value
sonar_filter_val = SONAR_MAXBOTIX12_BUTTER_NUM_1*sonar_meas
+ SONAR_MAXBOTIX12_BUTTER_NUM_2*sonar_meas_prev
// + SONAR_MAXBOTIX12_BUTTER_NUM_3*sonar_meas_prev_prev
- SONAR_MAXBOTIX12_BUTTER_DEN_2*sonar_filter_val_prev;
// - SONAR_MAXBOTIX12_BUTTER_DEN_3*sonar_filter_val_prev_prev;
sonar_meas_prev_prev = sonar_meas_prev;
sonar_meas_prev = sonar_meas;
sonar_filter_val_prev_prev = sonar_filter_val_prev;
sonar_filter_val_prev = sonar_filter_val;
/** using float
float factor_cm = 0.0165229885;
float alt_cm_flt = sonar_meas * factor_cm * (1<<8);
sonar_meas_real = alt_cm_flt;*/
//((69 / 72) / 58) * (2^16) = 1082.85057
//uint16_t conv_factor_cm = 1083;
//sonar_alt_cm_bfp = conv_factor_cm*sonar_filter_val;
//sonar_filtered = conv_factor_cm*sonar_filter_val;
//((69 / 72) / 58) / 100 * (2^8) = 0.0422988506
float conv_factor_m = 0.0422988506;
float alt_m_bfp_fl = conv_factor_m*sonar_filter_val;
sonar_filtered = alt_m_bfp_fl;
//ins_sonar_initialised = TRUE;
ins_update_sonar();
//sonar_filtered = conv_factor_cm*sonar_filter_val;
//DOWNLINK_SEND_VFF(DefaultChannel, &alt_mm_flt,0,0,0,0,0,0);
DOWNLINK_SEND_INS_Z(DefaultChannel, &sonar_filtered,&sonar_spike_cnt,0, 0);
}
}
static void send_ins_z(void) {
DOWNLINK_SEND_INS_Z(DefaultChannel, DefaultDevice,
&ins_impl.baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z);
}
static void send_ins_z(void) {
static const float fake_baro_z = 0.0;
DOWNLINK_SEND_INS_Z(DefaultChannel, DefaultDevice,
&fake_baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z);
}
