ardupilot/libraries/AP_Notify/GPIO_LED_1.cpp

80 lines
2.3 KiB
C++

/*
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 <http://www.gnu.org/licenses/>.
*/
#include "AP_Notify_config.h"
#if AP_NOTIFY_GPIO_LED_1_ENABLED
#include "GPIO_LED_1.h"
#include <AP_HAL/HAL.h>
#include "AP_Notify.h"
#ifndef AP_NOTIFY_GPIO_LED_1_PIN
#error "define AP_NOTIFY_GPIO_LED_1_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_1_PIN, HAL_GPIO_OUTPUT);
#endif
hal.gpio->write(AP_NOTIFY_GPIO_LED_1_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.failsafe_radio || AP_Notify::flags.failsafe_gcs || AP_Notify::flags.failsafe_battery) {
new_pattern = FAILSAFE;
} 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<<next_bit)) ? HAL_GPIO_LED_ON : HAL_GPIO_LED_OFF;
hal.gpio->write(AP_NOTIFY_GPIO_LED_1_PIN, new_state);
next_bit++;
if (next_bit > 31) {
next_bit = 0;
}
}
#endif // AP_NOTIFY_GPIO_LED_1_ENABLED