diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index da5ebde379..abfb047711 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -20,6 +20,7 @@ #include "Buzzer.h" #include "Display.h" #include "ExternalLED.h" +#include "GPIO_LED_1.h" #include "IS31FL3195.h" #include "PCA9685LED_I2C.h" #include "NavigatorLED.h" @@ -311,6 +312,9 @@ void AP_Notify::add_backends(void) ADD_BACKEND(NEW_NOTHROW AP_BoardLED()); #elif AP_NOTIFY_GPIO_LED_2_ENABLED ADD_BACKEND(NEW_NOTHROW AP_BoardLED2()); +#endif +#if AP_NOTIFY_GPIO_LED_1_ENABLED + ADD_BACKEND(NEW_NOTHROW GPIO_LED_1()); #endif break; #if AP_NOTIFY_TOSHIBALED_ENABLED diff --git a/libraries/AP_Notify/AP_Notify_config.h b/libraries/AP_Notify/AP_Notify_config.h index 23b7c6c8b1..0bc83f1905 100644 --- a/libraries/AP_Notify/AP_Notify_config.h +++ b/libraries/AP_Notify/AP_Notify_config.h @@ -55,6 +55,10 @@ #define AP_NOTIFY_GPIO_LED_2_ENABLED 0 #endif +#ifndef AP_NOTIFY_GPIO_LED_1_ENABLED +#define AP_NOTIFY_GPIO_LED_1_ENABLED 0 +#endif + #ifndef AP_NOTIFY_GPIO_LED_RGB_ENABLED #define AP_NOTIFY_GPIO_LED_RGB_ENABLED 0 #endif diff --git a/libraries/AP_Notify/GPIO_LED_1.cpp b/libraries/AP_Notify/GPIO_LED_1.cpp new file mode 100644 index 0000000000..950a1ce49c --- /dev/null +++ b/libraries/AP_Notify/GPIO_LED_1.cpp @@ -0,0 +1,77 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_Notify_config.h" + +#if AP_NOTIFY_GPIO_LED_1_ENABLED + +#include "GPIO_LED_1.h" + +#include +#include "AP_Notify.h" + +#ifndef AP_NOTIFY_GPIO_LED_A_PIN +#error "define AP_NOTIFY_GPIO_LED_A_PIN" +#endif + +extern const AP_HAL::HAL& hal; + +bool GPIO_LED_1::init(void) +{ + // when HAL_GPIO_LED_ON is 0 then we must not use pinMode() + // as it could remove the OPENDRAIN attribute on the pin +#if HAL_GPIO_LED_ON != 0 + hal.gpio->pinMode(AP_NOTIFY_GPIO_LED_A_PIN, HAL_GPIO_OUTPUT); +#endif + hal.gpio->write(AP_NOTIFY_GPIO_LED_A_PIN, HAL_GPIO_LED_OFF); + return true; +} + +/* + main update function called at 50Hz + */ +void GPIO_LED_1::update(void) +{ + uint32_t new_pattern; + if (AP_Notify::flags.initialising) { + new_pattern = INITIALIZING; + } else if (AP_Notify::flags.armed) { + new_pattern = ARMED; + } else if (AP_Notify::flags.pre_arm_check) { + new_pattern = READY_TO_ARM; + } else { + new_pattern = NOT_READY_TO_ARM; + } + if (new_pattern != current_pattern) { + next_bit = 0; + current_pattern = new_pattern; + last_timestep_ms = 0; + } + + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_timestep_ms < 100) { + return; + } + last_timestep_ms = now_ms; + + const auto new_state = (current_pattern & (1U<write(AP_NOTIFY_GPIO_LED_A_PIN, new_state); + next_bit++; + if (next_bit > 31) { + next_bit = 0; + } +} + +#endif // AP_NOTIFY_GPIO_LED_1_ENABLED diff --git a/libraries/AP_Notify/GPIO_LED_1.h b/libraries/AP_Notify/GPIO_LED_1.h new file mode 100644 index 0000000000..129e6cac52 --- /dev/null +++ b/libraries/AP_Notify/GPIO_LED_1.h @@ -0,0 +1,48 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include "AP_Notify_config.h" + +#if AP_NOTIFY_GPIO_LED_1_ENABLED + +#include +#include + +#include "NotifyDevice.h" + +class GPIO_LED_1 : public NotifyDevice +{ +public: + // initialise the LED driver + bool init(void) override; + + // should be called at 50Hz + void update(void) override; + +private: + + // left-to-right, each bit represents 100ms + static const uint32_t INITIALIZING = 0b10101010101010101010101010101010UL; + static const uint32_t NOT_READY_TO_ARM = 0b11111111000000001111111100000000UL; + static const uint32_t READY_TO_ARM = 0b11111111111111100000000000000000UL; + static const uint32_t ARMED = 0b11111111111111111111111111111111UL; + + uint32_t current_pattern = INITIALIZING; + uint32_t last_timestep_ms; + uint8_t next_bit; +}; + +#endif // AP_NOTIFY_GPIO_LED_1_ENABLED