123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267 |
- #include "AP_BoardLED2.h"
- #include "AP_Notify.h"
- #if defined(HAL_GPIO_A_LED_PIN) && defined(HAL_GPIO_B_LED_PIN)
- static_assert((HAL_GPIO_A_LED_PIN != HAL_GPIO_B_LED_PIN), "Duplicate LED assignments detected");
- extern const AP_HAL::HAL& hal;
- bool AP_BoardLED2::init(void)
- {
-
- hal.gpio->pinMode(HAL_GPIO_A_LED_PIN, HAL_GPIO_OUTPUT);
- hal.gpio->pinMode(HAL_GPIO_B_LED_PIN, HAL_GPIO_OUTPUT);
-
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
- hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
-
- _sat_cnt=0;
- save_trim_counter = 0;
- arm_counter = 0;
- return true;
- }
- void AP_BoardLED2::update(void)
- {
- _counter++;
-
- if (_counter % 3 != 0) {
- return;
- }
-
- uint8_t counter2 = _counter / 3;
-
- if (AP_Notify::flags.initialising) {
-
- hal.gpio->write(HAL_GPIO_A_LED_PIN, (counter2 & 1) ? HAL_GPIO_LED_ON : HAL_GPIO_LED_OFF);
- return;
- }
- if ((counter2 & 0x2) == 0) {
- save_trim_counter++;
- }
- bool led_a_used=false;
-
- if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
- switch(save_trim_counter) {
- case 0:
- hal.gpio->write(HAL_GPIO_B_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;
- default:
- save_trim_counter = -1;
- break;
- }
- return;
- }
- if(AP_Notify::flags.compass_cal_running){
- switch(save_trim_counter) {
- case 0:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
- hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_ON);
- break;
- case 1:
- case 2:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
- hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
- break;
- case 3:
- case 4:
- case 5:
- case 6:
- case 7:
- break;
- default:
-
- save_trim_counter = -1;
- break;
- }
- return;
-
- }
- if(AP_Notify::events.autotune_complete){
- switch(save_trim_counter) {
- case 0:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
- break;
- case 1:
- case 2:
- case 3:
- case 4:
- case 5:
- case 6:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
- break;
- case 7:
- break;
- default:
-
- save_trim_counter = -1;
- break;
- }
- led_a_used=true;
- }
- if(AP_Notify::events.autotune_failed){
- switch(save_trim_counter) {
- case 0:
- 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);
- break;
- case 2:
- case 3:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
- break;
- case 4:
- case 5:
- case 6:
- case 7:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
- break;
- default:
-
- save_trim_counter = -1;
- break;
- }
- led_a_used=true;
- }
-
- if(!led_a_used) {
- if (AP_Notify::flags.armed) {
- if(AP_Notify::flags.failsafe_battery){
- if ((counter2 & 0x7) == 0) {
- hal.gpio->toggle(HAL_GPIO_A_LED_PIN);
- }
- }else if(AP_Notify::flags.failsafe_radio){
- if ((counter2 & 0x3) == 0) {
- hal.gpio->toggle(HAL_GPIO_A_LED_PIN);
- }
- } else {
-
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
- }
- }else{
- if ((counter2 & 0x2) == 0) {
- arm_counter++;
- }
-
- if (AP_Notify::flags.pre_arm_check) {
-
- switch(arm_counter) {
- case 0:
- case 1:
- case 2:
- case 3:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
- break;
- case 4:
- case 5:
- case 6:
- case 7:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
- break;
- default:
-
- arm_counter = -1;
- break;
- }
- }else{
-
- switch(arm_counter) {
- case 0:
- case 1:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
- break;
-
- case 2:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
- break;
-
-
- case 3:
- case 4:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
- break;
-
- case 5:
- case 6:
- case 7:
- hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
- break;
-
- default:
- arm_counter = -1;
- break;
- }
- }
- }
- }
-
- switch (AP_Notify::flags.gps_status) {
- case 0:
- case 1:
-
- hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
- break;
- case 2:
- case 3:
- default:
- if ((counter2 & 0x2) == 0) {
- _sat_cnt++;
- }
- uint16_t sats = AP_Notify::flags.gps_num_sats;
-
- if(_sat_cnt<8) {
- hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
- } else if(_sat_cnt< (8 + (sats-6)*2) ) {
- hal.gpio->toggle(HAL_GPIO_B_LED_PIN);
- } else {
- _sat_cnt=-1;
- }
- break;
- }
- }
- #else
- bool AP_BoardLED2::init(void) {return true;}
- void AP_BoardLED2::update(void) {return;}
- #endif
|