src/lib/GSENSOR/devGsensor.cpp

   1 #include "targets.h"
   2 #include "common.h"
   3
   4 #include "devGsensor.h"
   5 #include <functional>
   6
   7 #include "handset.h"
   8 #include "gsensor.h"
   9 #include "POWERMGNT.h"
  10 #include "config.h"
  11 #include "logging.h"
  12
  13 Gsensor gsensor;
  14
  15 static int system_quiet_state = GSENSOR_SYSTEM_STATE_MOVING;
  16 static int system_quiet_pre_state = GSENSOR_SYSTEM_STATE_MOVING;
  17
  18 #define GSENSOR_DURATION    10
  19 #define GSENSOR_SYSTEM_IDLE_INTERVAL 1000U
  20
  21 #define MULTIPLE_BUMP_INTERVAL 400U
  22 #define BUMP_COMMAND_IDLE_TIME 10000U
  23
  24 static bool gSensorOk = false;
  25
  26 static void initialize()
  27 {
  28     if (OPT_HAS_GSENSOR)
  29     {
  30         gSensorOk = gsensor.init();
  31     }
  32 }
  33
  34 static int start()
  35 {
  36     if (OPT_HAS_GSENSOR && gSensorOk)
  37     {
  38         return DURATION_IMMEDIATELY;
  39     }
  40     return DURATION_NEVER;
  41 }
  42
  43 static int timeout()
  44 {
  45     static unsigned long lastIdleCheckMs = 0;
  46     unsigned long now = millis();
  47     if (now - lastIdleCheckMs > GSENSOR_SYSTEM_IDLE_INTERVAL)
  48     {
  49         gsensor.handle();
  50
  51         system_quiet_state = gsensor.getSystemState();
  52         //When system is idle, set power to minimum
  53         if(config.GetMotionMode() == 1)
  54         {
  55             if((system_quiet_state == GSENSOR_SYSTEM_STATE_QUIET) && (system_quiet_pre_state == GSENSOR_SYSTEM_STATE_MOVING) && !handset->IsArmed())
  56             {
  57                 POWERMGNT::setPower(MinPower);
  58             }
  59             if((system_quiet_state == GSENSOR_SYSTEM_STATE_MOVING) && (system_quiet_pre_state == GSENSOR_SYSTEM_STATE_QUIET) && POWERMGNT::currPower() < (PowerLevels_e)config.GetPower())
  60             {
  61                 POWERMGNT::setPower((PowerLevels_e)config.GetPower());
  62             }
  63         }
  64         system_quiet_pre_state = system_quiet_state;
  65         lastIdleCheckMs = now;
  66     }
  67     return GSENSOR_DURATION;
  68 }
  69
  70 device_t Gsensor_device = {
  71     .initialize = initialize,
  72     .start = start,
  73     .event = NULL,
  74     .timeout = timeout
  75 };