src/lib/GSENSOR/gsensor.cpp

   1 #include "targets.h"
   2
   3 #include "gsensor.h"
   4 #include "logging.h"
   5
   6 #include "stk8baxx.h"
   7 STK8xxx stk8xxx;
   8
   9 int gensor_status = GSENSOR_STATUS_FAIL;
  10
  11 #define DATA_SAMPLE_LENGTH  20
  12 #define QUIET_VARIANCE_THRESHOLD    0.0002f
  13 #define QUIET_AVERAGE_THRESHOLD     0.04f
  14
  15 #define FLIPPED_ACTION_THRESHOLD  0.2f
  16
  17 static float x_buffer[DATA_SAMPLE_LENGTH] = {0};
  18 static float y_buffer[DATA_SAMPLE_LENGTH] = {0};
  19 static float z_buffer[DATA_SAMPLE_LENGTH] = {0};
  20 static int sample_counter = 0;
  21 float x_average = 0;
  22 float y_average = 0;
  23 float z_average = 0;
  24
  25 static bool interrupt = false;
  26
  27 #ifdef HAS_SMART_FAN
  28 extern bool is_smart_fan_control;
  29 uint32_t smart_fan_start_time = 0;
  30 #define SMART_FAN_TIME_OUT 5000
  31 #endif
  32
  33 ICACHE_RAM_ATTR void handleGsensorInterrupt()
  34 {
  35     interrupt = true;
  36 }
  37
  38 bool Gsensor::init()
  39 {
  40     int16_t id = -1;
  41     if (OPT_HAS_GSENSOR_STK8xxx)
  42         id = stk8xxx.STK8xxx_Initialization();
  43     else
  44         return false;
  45
  46     if(id == -1)
  47     {
  48         ERRLN("Gsensor failed!");
  49         return false;
  50     }
  51     else
  52     {
  53         DBGLN("Gsensor OK with chipid = %x!", id);
  54         gensor_status = GSENSOR_STATUS_NORMAL;
  55     }
  56
  57     if(gensor_status == GSENSOR_STATUS_NORMAL)
  58     {
  59         if (OPT_HAS_GSENSOR_STK8xxx)
  60         {
  61             stk8xxx.STK8xxx_Anymotion_init();
  62             pinMode(GPIO_PIN_GSENSOR_INT,INPUT_PULLUP);
  63             attachInterrupt(digitalPinToInterrupt(GPIO_PIN_GSENSOR_INT), handleGsensorInterrupt, FALLING);
  64         }
  65     }
  66
  67     system_state = GSENSOR_SYSTEM_STATE_MOVING;
  68     is_flipped = false;
  69     return true;
  70 }
  71
  72 float get_data_average(float *data, int length)
  73 {
  74     float average = 0;
  75     for(int i=0;i<length;i++)
  76     {
  77         average += *(data+i);
  78     }
  79     average = average / length;
  80     return average;
  81 }
  82
  83 float get_data_variance(float average, float *data, int length)
  84 {
  85     float variance = 0;
  86     for(int i=0;i<length;i++)
  87     {
  88         float temp = *(data+i) - average;
  89         temp = temp * temp;
  90         variance += temp;
  91     }
  92     variance = variance / length;
  93     return variance;
  94 }
  95
  96 bool Gsensor::hasTriggered(unsigned long now)
  97 {
  98     static unsigned long lastTriggeredMs = 0;
  99
 100     if (interrupt)
 101     {
 102         interrupt = false;
 103         if (now - lastTriggeredMs > 20)
 104         {
 105             lastTriggeredMs = now;
 106             return true;
 107         }
 108     }
 109     return false;
 110 }
 111
 112 void Gsensor::handle()
 113 {
 114     float x, y, z;
 115
 116     getGSensorData(&x, &y, &z);
 117 #ifdef HAS_SMART_FAN
 118     if(z < -0.5f)
 119     {
 120         is_smart_fan_control = true;
 121         smart_fan_start_time = millis();
 122     }
 123     else
 124     {
 125         if(is_smart_fan_control && (millis() - smart_fan_start_time) > SMART_FAN_TIME_OUT)
 126         {
 127             is_smart_fan_control = false;
 128         }
 129     }
 130 #endif
 131     if(z > FLIPPED_ACTION_THRESHOLD)
 132     {
 133         is_flipped = true;
 134     }
 135     else
 136     {
 137         is_flipped = false;
 138     }
 139     if(system_state == GSENSOR_SYSTEM_STATE_QUIET)
 140     {
 141         x = abs(x - x_average);
 142         y = abs(y - y_average);
 143         z = abs(z - z_average);
 144         if((x > QUIET_AVERAGE_THRESHOLD) || (y > QUIET_AVERAGE_THRESHOLD) ||
 145             (z > QUIET_AVERAGE_THRESHOLD))
 146         {
 147             system_state = GSENSOR_SYSTEM_STATE_MOVING;
 148         }
 149     }
 150     else
 151     {
 152         x_buffer[sample_counter] = x;
 153         y_buffer[sample_counter] = y;
 154         z_buffer[sample_counter] = z;
 155
 156         if(sample_counter == DATA_SAMPLE_LENGTH -1)
 157         {
 158             sample_counter = 0;
 159             x_average = get_data_average(x_buffer, DATA_SAMPLE_LENGTH);
 160             y_average = get_data_average(y_buffer, DATA_SAMPLE_LENGTH);
 161             z_average = get_data_average(z_buffer, DATA_SAMPLE_LENGTH);
 162
 163             float x_variance = get_data_variance(x_average, x_buffer, DATA_SAMPLE_LENGTH);
 164             float y_variance = get_data_variance(y_average, y_buffer, DATA_SAMPLE_LENGTH);
 165             float z_variance = get_data_variance(z_average, z_buffer, DATA_SAMPLE_LENGTH);
 166
 167             if((x_variance < QUIET_VARIANCE_THRESHOLD) && (y_variance < QUIET_VARIANCE_THRESHOLD) &&
 168                 (z_variance < QUIET_VARIANCE_THRESHOLD))
 169             {
 170                 system_state = GSENSOR_SYSTEM_STATE_QUIET;
 171             }
 172         }
 173         else
 174         {
 175             sample_counter++;
 176         }
 177     }
 178 }
 179
 180 void Gsensor::getGSensorData(float *X_DataOut, float *Y_DataOut, float *Z_DataOut)
 181 {
 182     *X_DataOut = 0;
 183     *Y_DataOut = 0;
 184     *Z_DataOut = 0;
 185     if(gensor_status == GSENSOR_STATUS_NORMAL)
 186     {
 187         if (OPT_HAS_GSENSOR_STK8xxx)
 188             stk8xxx.STK8xxx_Getregister_data(X_DataOut, Y_DataOut, Z_DataOut);
 189     }
 190     else
 191     {
 192         ERRLN("Gsensor abnormal status = %d", gensor_status);
 193     }
 194 }
 195
 196 int Gsensor::getSystemState()
 197 {
 198     return system_state;
 199 }
 200
 201 bool Gsensor::isFlipped()
 202 {
 203     return is_flipped;
 204 }