src/lib/GSENSOR/devGsensor.cpp

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