diff --git a/libraries/AP_Notify/AP_BoardLED.cpp b/libraries/AP_Notify/AP_BoardLED.cpp new file mode 100644 index 0000000000..798c50fbeb --- /dev/null +++ b/libraries/AP_Notify/AP_BoardLED.cpp @@ -0,0 +1,160 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +extern const AP_HAL::HAL& hal; + +// static private variable instantiation +uint16_t AP_BoardLED::_counter; + +void AP_BoardLED::init(void) +{ + // update LEDs as often as needed + hal.scheduler->register_timer_process( AP_BoardLED::_update ); + + // setup the main LEDs as outputs + hal.gpio->pinMode(HAL_GPIO_A_LED_PIN, GPIO_OUTPUT); + hal.gpio->pinMode(HAL_GPIO_B_LED_PIN, GPIO_OUTPUT); + hal.gpio->pinMode(HAL_GPIO_C_LED_PIN, GPIO_OUTPUT); +} + +void AP_BoardLED::_update(uint32_t now) +{ + _counter++; + + // we never want to update LEDs at a higher than 16Hz rate + if (_counter & 0x3F) { + return; + } + + // counter2 used to drop frequency down to 16hz + uint8_t counter2 = _counter >> 6; + + // initialising + if (notify.flags.initialising) { + // blink LEDs A and C at 8Hz (full cycle) during initialisation + if (counter2 & 1) { + hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON); + hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_OFF); + } else { + hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF); + hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_ON); + } + return; + } + + // save trim + if (notify.flags.save_trim) { + 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; + } + return; + } + + // arming light + static uint8_t arm_counter = 0; + if (notify.flags.armed) { + // red led solid + hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON); + }else{ + if ((counter2 & 0x2) == 0) { + arm_counter++; + } + if (notify.flags.pre_arm_check) { + // 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; + } + // disarmed and passing pre-arm checks, blink at about 2hz + //if ((counter2 & 0x7) == 0) { + // hal.gpio->toggle(HAL_GPIO_A_LED_PIN); + //} + }else{ + // disarmed and failing pre-arm checks, double blink + //if (counter2 & 0x4) { + // hal.gpio->toggle(HAL_GPIO_A_LED_PIN); + //} + // failed pre-arm checks so double flash + 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: + hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF); + break; + default: + arm_counter = -1; + break; + } + } + } + + // gps light + switch (notify.flags.gps_status) { + 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; + + case 3: + // GPS attached but 2D lock, blink more slowly (around 2Hz) + hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_ON); + break; + } +} diff --git a/libraries/AP_Notify/AP_BoardLED.h b/libraries/AP_Notify/AP_BoardLED.h new file mode 100644 index 0000000000..58b2e43eae --- /dev/null +++ b/libraries/AP_Notify/AP_BoardLED.h @@ -0,0 +1,60 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef __AP_HAL_BOARDLED_H__ +#define __AP_HAL_BOARDLED_H__ + +#include +#include +#include + +#define HIGH 1 +#define LOW 0 + +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 + # define HAL_GPIO_A_LED_PIN 37 + # define HAL_GPIO_B_LED_PIN 36 + # define HAL_GPIO_C_LED_PIN 35 + # define HAL_GPIO_LED_ON HIGH + # define HAL_GPIO_LED_OFF LOW +#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 + # define HAL_GPIO_A_LED_PIN 27 + # define HAL_GPIO_B_LED_PIN 26 + # define HAL_GPIO_C_LED_PIN 25 + # define HAL_GPIO_LED_ON LOW + # define HAL_GPIO_LED_OFF HIGH +#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL + # define HAL_GPIO_A_LED_PIN 27 + # define HAL_GPIO_B_LED_PIN 26 + # define HAL_GPIO_C_LED_PIN 25 + # define HAL_GPIO_LED_ON LOW + # define HAL_GPIO_LED_OFF HIGH +#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 + # define HAL_GPIO_A_LED_PIN 27 + # define HAL_GPIO_B_LED_PIN 26 + # define HAL_GPIO_C_LED_PIN 25 + # define HAL_GPIO_LED_ON LOW + # define HAL_GPIO_LED_OFF HIGH +#elif CONFIG_HAL_BOARD == HAL_BOARD_SMACCM +// XXX these are just copied, may not make sense + # define HAL_GPIO_A_LED_PIN 27 + # define HAL_GPIO_B_LED_PIN 26 + # define HAL_GPIO_C_LED_PIN 25 + # define HAL_GPIO_LED_ON LOW + # define HAL_GPIO_LED_OFF HIGH +#endif + +class AP_BoardLED +{ +public: + // initialise the LED driver + void init(void); + +private: + // private methods + static void _update(uint32_t now); + + // private member variables + static uint16_t _counter; +}; + +#endif // __AP_HAL_BOARDLED_H__ diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp new file mode 100644 index 0000000000..88b4084675 --- /dev/null +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -0,0 +1,8 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include + +AP_Notify notify; +/// Constructor +//AP_Notify::AP_Notify() +//{ +//} diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h new file mode 100644 index 0000000000..92dc9853d4 --- /dev/null +++ b/libraries/AP_Notify/AP_Notify.h @@ -0,0 +1,55 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef __AP_NOTIFY_H__ +#define __AP_NOTIFY_H__ + +#include + +class AP_Notify +{ +public: + + /// notify_type - bitmask of notification types + struct notify_type { + uint16_t initialising : 1; // 1 if initialising and copter should not be moved + uint16_t gps_status : 2; // 0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock + uint16_t armed : 1; // 0 = disarmed, 1 = armed + uint16_t pre_arm_check : 1; // 0 = failing checks, 1 = passed + uint16_t save_trim : 1; // 1 if gathering trim data + uint16_t esc_calibration: 1; // 1 if calibrating escs + } flags; + + /// Constructor + //Notify(); + + /// To-Do: potential notifications to add + + /// flight_mode + /// void flight_mode(uint8_t mode) = 0; + + /// throttle failsafe + /// void fs_throttle(bool uint8_t); // 0 if throttle failsafe is cleared, 1 if activated + /// void fs_battery(bool uint8_t); // 1 if battery voltage is low or consumed amps close to battery capacity, 0 if cleared + /// void fs_gps(bool uint8_t); // 1 if we've lost gps lock and it is required for our current flightmode, 0 if cleared + /// void fs_gcs(bool uint8_t); // 1 if we've lost contact with the gcs and it is required for our current flightmode or pilot input method, 0 if cleared + /// void fence_breach(bool uint8_t); // fence type breached or 0 if cleared + + /// switch_aux1(uint8_t state); // 0 if aux switch is off, 1 if in middle, 2 if high + /// switch_aux2(uint8_t state); // 0 if aux switch is off, 1 if in middle, 2 if high + + /// reached_waypoint(); // called after we reach a waypoint + + /// error(uint8_t subsystem_id, uint8_t error_code); // general error reporting + + /// objects that we expect to create: + /// apm2, px4 leds + /// copter leds + /// blinkm + /// buzzer + +}; + +// declare a static instance so that it can be accessed easily from anywhere +extern AP_Notify notify; + +#endif // __AP_NOTIFY_H__ diff --git a/libraries/AP_Notify/examples/AP_Notify_test/AP_Notify_test.pde b/libraries/AP_Notify/examples/AP_Notify_test/AP_Notify_test.pde new file mode 100644 index 0000000000..4e7bbda985 --- /dev/null +++ b/libraries/AP_Notify/examples/AP_Notify_test/AP_Notify_test.pde @@ -0,0 +1,39 @@ +/* + * Example of AC_Notify library . + * DIYDrones.com + */ + +#include +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include +#include +#include +#include // Notify library +#include // Board LED library + +const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; + +// create board led object +AP_BoardLED board_led; + +void setup() +{ + hal.console->println("AP_Notify library test"); + + // initialise the board leds + board_led.init(); + + // turn on initialising notification + //notify.flags.initialising = true; + notify.flags.gps_status = 1; + notify.flags.armed = 1; + notify.flags.pre_arm_check = 1; +} + +void loop() +{ + hal.scheduler->delay(1000); +} + +AP_HAL_MAIN(); diff --git a/libraries/AP_Notify/examples/AP_Notify_test/Makefile b/libraries/AP_Notify/examples/AP_Notify_test/Makefile new file mode 100644 index 0000000000..f786b89085 --- /dev/null +++ b/libraries/AP_Notify/examples/AP_Notify_test/Makefile @@ -0,0 +1 @@ +include ../../../../mk/apm.mk diff --git a/libraries/AP_Notify/examples/AP_Notify_test/nocore.inoflag b/libraries/AP_Notify/examples/AP_Notify_test/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2