diff --git a/libraries/AP_HAL/board/sitl.h b/libraries/AP_HAL/board/sitl.h index a20951e0d06ca0..0eae8e932a3c1b 100644 --- a/libraries/AP_HAL/board/sitl.h +++ b/libraries/AP_HAL/board/sitl.h @@ -48,6 +48,9 @@ #define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 9 #define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 10 +// #define AP_NOTIFY_GPIO_LED_1_ENABLED 1 +#define AP_NOTIFY_GPIO_LED_A_PIN 8 // these are set in SIM_PIN_MASK + #define HAL_HAVE_BOARD_VOLTAGE 1 #define HAL_HAVE_SERVO_VOLTAGE 1 #define HAL_HAVE_SAFETY_SWITCH 1 diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index a74655db832ef3..555331501add04 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" @@ -313,6 +314,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 62238d28ed09e0..de72773bb4924d 100644 --- a/libraries/AP_Notify/AP_Notify_config.h +++ b/libraries/AP_Notify/AP_Notify_config.h @@ -47,6 +47,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 00000000000000..950a1ce49cad2f --- /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 00000000000000..129e6cac520185 --- /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 diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 366ec909304ace..a6baf91eb9da24 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -1094,6 +1094,9 @@ void Aircraft::update_external_payload(const struct sitl_input &input) } #endif +#if AP_SIM_GPIO_LED_1_ENABLED + sim_led1.update(*this); +#endif #if AP_SIM_GPIO_LED_2_ENABLED sim_led2.update(*this); #endif diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 4af43bb80cd092..90f07089ea60b1 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -38,6 +38,7 @@ #include #include "SIM_JSON_Master.h" #include "ServoModel.h" +#include "SIM_GPIO_LED_1.h" #include "SIM_GPIO_LED_2.h" #include "SIM_GPIO_LED_3.h" #include "SIM_GPIO_LED_RGB.h" @@ -377,6 +378,9 @@ class Aircraft { #endif +#if AP_SIM_GPIO_LED_1_ENABLED + GPIO_LED_1 sim_led1{8}; // pin to match sitl.h +#endif #if AP_SIM_GPIO_LED_2_ENABLED GPIO_LED_2 sim_led2{13, 14}; // pins to match sitl.h #endif diff --git a/libraries/SITL/SIM_GPIO_LED_1.cpp b/libraries/SITL/SIM_GPIO_LED_1.cpp new file mode 100644 index 00000000000000..5ae8a40ef4d3da --- /dev/null +++ b/libraries/SITL/SIM_GPIO_LED_1.cpp @@ -0,0 +1,31 @@ +#include "SIM_config.h" + +#if AP_SIM_GPIO_LED_1_ENABLED + +#include "SIM_GPIO_LED_1.h" + +#include + +using namespace SITL; + +void GPIO_LED_1::init() +{ + leds.init(); +} + +void GPIO_LED_1::update(const class Aircraft &aircraft) +{ + if (!init_done) { + init(); + init_done = true; + } + + const uint16_t pin_mask = AP::sitl()->pin_mask.get(); + const bool new_led_states[1] { + ((pin_mask & uint16_t((1U<::LEDColour colours[1] { + SIM_LED_n<1>::LEDColour::RED, + }; + + SIM_LED_n<1> leds{"GPIO_LED_1", colours}; + + uint8_t LED_A_PIN; +}; + +} // namespace SITL + +#endif // AP_SIM_GPIO_LED_2_ENABLED diff --git a/libraries/SITL/SIM_LED_n.cpp b/libraries/SITL/SIM_LED_n.cpp index 30cd3b90efde9c..df721612f79275 100644 --- a/libraries/SITL/SIM_LED_n.cpp +++ b/libraries/SITL/SIM_LED_n.cpp @@ -90,6 +90,7 @@ void SIM_LED_n::init() pthread_create(&thread, NULL, update_thread_start, this); } +template class SIM_LED_n<1>; template class SIM_LED_n<2>; template class SIM_LED_n<3>; diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 99a53bfb0fd437..bd5d07e799c50f 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -27,6 +27,10 @@ #define AP_SIM_LED_N_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) && defined(WITH_SITL_RGBLED) #endif +#ifndef AP_SIM_GPIO_LED_1_ENABLED +#define AP_SIM_GPIO_LED_1_ENABLED AP_SIM_LED_N_ENABLED && 0 +#endif + #ifndef AP_SIM_GPIO_LED_2_ENABLED #define AP_SIM_GPIO_LED_2_ENABLED AP_SIM_LED_N_ENABLED && 0 #endif @@ -36,7 +40,7 @@ #endif #ifndef AP_SIM_GPIO_LED_RGB_ENABLED -#define AP_SIM_GPIO_LED_RGB_ENABLED AP_SIM_LED_N_ENABLED +#define AP_SIM_GPIO_LED_RGB_ENABLED AP_SIM_LED_N_ENABLED && 0 #endif #ifndef AP_SIM_LOWEHEISER_ENABLED