diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 6de3910..6eaffa4 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -79,6 +79,8 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){ zl = 0; zh = 0; tilt_time = 0; + tmp_tilt_status = 0; + tilt_filter = 0; // version version_high = VERSION_BYTE_HIGH; @@ -714,13 +716,24 @@ void Arduino_AlvikCarrier::updateImu(){ imu->Get_6D_Orientation_ZL(&zl); imu->Get_6D_Orientation_ZH(&zh); - tilt_status = 0; - tilt_status |= xl<<4; - tilt_status |= xh<<5; - tilt_status |= zl<<7; - tilt_status |= zh<<6; - tilt_status |= yh<<3; - tilt_status |= yl<<2; + tmp_tilt_status = 0; + tmp_tilt_status |= xl<<4; + tmp_tilt_status |= xh<<5; + tmp_tilt_status |= zl<<7; + tmp_tilt_status |= zh<<6; + tmp_tilt_status |= yh<<3; + tmp_tilt_status |= yl<<2; + + if (tilt_status == tmp_tilt_status){ + tilt_filter++; + }else{ + tilt_filter = 0; + } + + if (tilt_filter>20){ + tilt_status = tmp_tilt_status; + } + } } diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 421f771..2388075 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -73,6 +73,8 @@ class Arduino_AlvikCarrier{ uint8_t tilt_status; uint8_t xl, xh, yl, yh, zl, zh; unsigned long tilt_time; + uint8_t tilt_filter; + uint8_t tmp_tilt_status; uint8_t version_high; uint8_t version_mid;