ardupilot/libraries/AP_Notify/AP_BoardLED.cpp

171 lines
5.2 KiB
C++
Raw Normal View History

/*
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.
2013-08-08 10:13:14 -03:00
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_BoardLED.h"
2013-08-08 10:13:14 -03:00
#include "AP_Notify.h"
#if (defined(HAL_GPIO_A_LED_PIN) && defined(HAL_GPIO_B_LED_PIN) && \
defined(HAL_GPIO_C_LED_PIN))
extern const AP_HAL::HAL& hal;
2013-08-08 10:13:14 -03:00
bool AP_BoardLED::init(void)
2013-08-08 10:13:14 -03:00
{
// setup the main LEDs as outputs
2014-06-01 20:27:01 -03:00
hal.gpio->pinMode(HAL_GPIO_A_LED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->pinMode(HAL_GPIO_B_LED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->pinMode(HAL_GPIO_C_LED_PIN, HAL_GPIO_OUTPUT);
2013-10-28 22:53:36 -03:00
// turn all lights off
hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_OFF);
return true;
2013-08-08 10:13:14 -03:00
}
/*
main update function called at 50Hz
*/
void AP_BoardLED::update(void)
2013-08-08 10:13:14 -03:00
{
_counter++;
// we never want to update LEDs at a higher than 16Hz rate
if (_counter % 3 != 0) {
2013-08-08 10:13:14 -03:00
return;
}
// counter2 used to drop frequency down to 16hz
uint8_t counter2 = _counter / 3;
2013-08-08 10:13:14 -03:00
// initialising
2013-08-13 23:50:52 -03:00
if (AP_Notify::flags.initialising) {
2013-08-08 10:13:14 -03:00
// blink LEDs A and C at 8Hz (full cycle) during initialisation
hal.gpio->write(HAL_GPIO_A_LED_PIN, (counter2 & 1) ? HAL_GPIO_LED_ON : HAL_GPIO_LED_OFF);
hal.gpio->write(HAL_GPIO_C_LED_PIN, (counter2 & 1) ? HAL_GPIO_LED_OFF : HAL_GPIO_LED_ON);
2013-08-08 10:13:14 -03:00
return;
}
// save trim and ESC calibration
if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
2013-08-08 10:13:14 -03:00
static uint8_t save_trim_counter = 0;
if ((counter2 & 0x2) == 0) {
save_trim_counter++;
}
switch(save_trim_counter) {
case 0:
hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_OFF);
hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
break;
case 1:
hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_ON);
break;
case 2:
hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_ON);
break;
default:
save_trim_counter = -1;
break;
2013-08-08 10:13:14 -03:00
}
return;
}
// arming light
static uint8_t arm_counter = 0;
2013-08-13 23:50:52 -03:00
if (AP_Notify::flags.armed) {
2013-08-08 10:13:14 -03:00
// red led solid
hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
}else{
if ((counter2 & 0x2) == 0) {
arm_counter++;
}
2013-08-13 23:50:52 -03:00
if (AP_Notify::flags.pre_arm_check) {
2013-08-08 10:13:14 -03:00
// passed pre-arm checks so slower single flash
switch(arm_counter) {
case 0:
case 1:
case 2:
hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
break;
case 3:
case 4:
case 5:
hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
break;
default:
// reset counter to restart the sequence
arm_counter = -1;
break;
}
}else{
// failed pre-arm checks so double flash
switch(arm_counter) {
case 0:
case 1:
case 3:
case 4:
hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
break;
case 2:
2013-08-08 10:13:14 -03:00
case 5:
case 6:
hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
break;
2013-08-08 10:13:14 -03:00
default:
arm_counter = -1;
break;
}
}
}
// gps light
2013-08-13 23:50:52 -03:00
switch (AP_Notify::flags.gps_status) {
2013-08-08 10:13:14 -03:00
case 0:
// no GPS attached
hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_OFF);
break;
case 1:
// GPS attached but no lock, blink at 4Hz
if ((counter2 & 0x3) == 0) {
hal.gpio->toggle(HAL_GPIO_C_LED_PIN);
}
break;
case 2:
// GPS attached but 2D lock, blink more slowly (around 2Hz)
if ((counter2 & 0x7) == 0) {
hal.gpio->toggle(HAL_GPIO_C_LED_PIN);
}
break;
default:
2013-08-13 23:50:52 -03:00
// solid blue on gps lock
2013-08-08 10:13:14 -03:00
hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_ON);
break;
}
}
#else
bool AP_BoardLED::init(void) {return true;}
void AP_BoardLED::update(void) {return;}
#endif