/**
* @brief Read out newest sonar data
*
* @param sonar_value_filtered Filtered return value
* @param sonar_value_raw Raw return value
*/
bool sonar_read(float* sonar_value_filtered, float* sonar_value_raw)
{
// check SR04 timeout
if (sr04_initialized && trigger_time > 0 && get_boot_time_us() - trigger_time > SONAR_TIMEOUT)
{
printf("TIMEOUT\n");
sonar_valid = false;
trigger_time = 0;
}
if (private_sonar_initialized && timeout && isnan(distance))
{
sonar_valid = false;
}
/* getting new data with only around 10Hz */
if (new_value) {
sonar_filter();
new_value = 0;
sonar_measure_time = get_boot_time_us();
}
/* catch post-filter out of band values */
if (x_post < SONAR_MIN || x_post > SONAR_MAX) {
sonar_valid = false;
}
*sonar_value_filtered = x_post;
*sonar_value_raw = sonar_raw;
return sonar_valid;
}
/****************定高数据处理**************/
void AltHold_Data_Handling()
{
float high=0;
//获取滤波后的超声波高度 //ps :注意滤波后的相位不要太滞后
//判断声纳原始数据是否有效
sonar_valid=Sonar_Valid_Check(sonar_valid,&Ultra);
high=sonar_filter(&sonar_valid,&Ultra)*100;//cm
if(high==0)
{
sonar_valid=false;
}
else if(sonar_valid==true)
{
Ultra.z=high;
}
}
/**
* @brief Read out newest sonar data
*
* @param sonar_value_filtered Filtered return value
* @param sonar_value_raw Raw return value
*/
bool sonar_read(float* sonar_value_filtered, float* sonar_value_raw)
{
/* getting new data with only around 10Hz */
if (new_value) {
sonar_filter();
new_value = 0;
sonar_measure_time = get_boot_time_us();
}
/* catch post-filter out of band values */
if (x_post < SONAR_MIN || x_post > SONAR_MAX) {
sonar_valid = false;
}
*sonar_value_filtered = x_post;
*sonar_value_raw = sonar_raw;
return sonar_valid;
}
