src/main/io/lights.c

   1 #include "stdbool.h"
   2 #include "stdint.h"
   3 #include "stdlib.h"
   4
   5 #include "platform.h"
   6
   7 #include "common/utils.h"
   8
   9 #include "drivers/time.h"
  10 #include "drivers/lights_io.h"
  11
  12 #include "rx/rx.h"
  13
  14 #include "fc/config.h"
  15 #include "fc/rc_controls.h"
  16 #include "fc/rc_modes.h"
  17 #include "fc/runtime_config.h"
  18
  19 #include "scheduler/scheduler.h"
  20
  21 #include "config/feature.h"
  22
  23 #include "config/parameter_group.h"
  24 #include "config/parameter_group_ids.h"
  25
  26 #include "io/lights.h"
  27
  28 #ifdef USE_LIGHTS
  29
  30 PG_REGISTER_WITH_RESET_TEMPLATE(lightsConfig_t, lightsConfig, PG_LIGHTS_CONFIG, 0);
  31
  32 PG_RESET_TEMPLATE(lightsConfig_t, lightsConfig,
  33         .failsafe = {
  34             .enabled = SETTING_FAILSAFE_LIGHTS_DEFAULT,
  35             .flash_period = SETTING_FAILSAFE_LIGHTS_FLASH_PERIOD_DEFAULT,
  36             .flash_on_time = SETTING_FAILSAFE_LIGHTS_FLASH_ON_TIME_DEFAULT
  37         }
  38 );
  39
  40 static bool lights_on = false;
  41 static timeUs_t last_status_change = 0;
  42
  43 static void lightsSetStatus(bool status, timeUs_t currentTimeUs)
  44 {
  45     if (status != lights_on) {
  46         lights_on = status;
  47         lightsHardwareSetStatus(status);
  48         last_status_change = currentTimeUs;
  49     }
  50 }
  51
  52 /*
  53  * Lights handler function to be called periodically in loop. Updates lights
  54  * state via time schedule.
  55  */
  56 void lightsUpdate(timeUs_t currentTimeUs)
  57 {
  58     UNUSED(currentTimeUs);
  59     if (lightsConfig()->failsafe.enabled && FLIGHT_MODE(FAILSAFE_MODE) && ARMING_FLAG(WAS_EVER_ARMED)) {
  60         if (lightsConfig()->failsafe.flash_period <= lightsConfig()->failsafe.flash_on_time) {
  61             lightsSetStatus(true, currentTimeUs);
  62         } else {
  63             if (lights_on) {
  64                 if (currentTimeUs - last_status_change > lightsConfig()->failsafe.flash_on_time * 1000)
  65                     lightsSetStatus(false, currentTimeUs);
  66             } else {
  67                 if (currentTimeUs - last_status_change > (uint32_t)(lightsConfig()->failsafe.flash_period - lightsConfig()->failsafe.flash_on_time) * 1000)
  68                     lightsSetStatus(true, currentTimeUs);
  69             }
  70         }
  71     } else
  72         lightsSetStatus(IS_RC_MODE_ACTIVE(BOXLIGHTS), currentTimeUs);
  73 }
  74
  75 void lightsInit(void)
  76 {
  77     lightsHardwareInit();
  78 }
  79
  80 #endif /* USE_LIGHTS */