diff --git a/libraries/AP_Notify/OreoLED_PX4.cpp b/libraries/AP_Notify/OreoLED_PX4.cpp index 5bb6e3f3a3..e0f927a8cc 100644 --- a/libraries/AP_Notify/OreoLED_PX4.cpp +++ b/libraries/AP_Notify/OreoLED_PX4.cpp @@ -21,33 +21,35 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #include #include "OreoLED_PX4.h" - #include #include #include #include #include - #include #include #include - #include "AP_Notify.h" -extern const AP_HAL::HAL& hal; - #define OREOLED_BACKLEFT 0 // back left led instance number #define OREOLED_BACKRIGHT 1 // back right led instance number #define OREOLED_FRONTRIGHT 2 // front right led instance number #define OREOLED_FRONTLEFT 3 // front left led instance number +#define PERIOD_SLOW 800 // slow flash rate +#define PERIOD_FAST 500 // fast flash rate +#define PERIOD_SUPER 150 // super fast rate +#define PO_ALTERNATE 180 // 180 degree phase offset + +extern const AP_HAL::HAL& hal; // constructor -OreoLED_PX4::OreoLED_PX4() : NotifyDevice(), +OreoLED_PX4::OreoLED_PX4(uint8_t theme): NotifyDevice(), _overall_health(false), _oreoled_fd(-1), _send_required(false), _state_desired_semaphore(false), - _pattern_override(0) + _pattern_override(0), + _oreo_theme(theme) { // initialise desired and sent state memset(_state_desired,0,sizeof(_state_desired)); @@ -56,9 +58,13 @@ OreoLED_PX4::OreoLED_PX4() : NotifyDevice(), extern "C" int oreoled_main(int, char **); -// init - initialised the device + +// +// Initialize the LEDs +// bool OreoLED_PX4::init() { + #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) if (!AP_BoardConfig::px4_start_driver(oreoled_main, "oreoled", "start autoupdate")) { hal.console->printf("Unable to start oreoled driver\n"); @@ -67,7 +73,7 @@ bool OreoLED_PX4::init() hal.scheduler->delay(500); } #endif - + // open the device _oreoled_fd = open(OREOLED0_DEVICE_PATH, O_RDWR); if (_oreoled_fd == -1) { @@ -84,163 +90,270 @@ bool OreoLED_PX4::init() return _overall_health; } -// update - updates device according to timed_updated. Should be -// called at 50Hz + +// UPDATE device according to timed_updated. Called at 50Hz void OreoLED_PX4::update() { - static uint8_t counter = 0; // counter to reduce rate from 50hz to 10hz - static uint8_t step = 0; // step to control pattern - static uint8_t last_stage = 0; // unique id of the last messages sent to the LED, used to reduce resends which disrupt some patterns - static uint8_t initialization_done = 0; // Keep track if initialization has begun. There is a period when the driver - // is running but initialization has not yet begun -- this prevents post-initialization - // LED patterns from displaying before initialization has completed. - // return immediately if not healthy - if (!_overall_health) { - return; - } + if (!_overall_health) { return; } // don't go any further if LED driver reports unhealthy - // handle firmware update event - if (AP_Notify::flags.firmware_update) { - // Force a syncronisation before setting the free-running colour cycle macro - send_sync(); - set_macro(OREOLED_INSTANCE_ALL, OREOLED_PARAM_MACRO_COLOUR_CYCLE); - return; - } + if (slow_counter()) { return; } // slow rate from 50hz to 10hz - // return immediately if custom pattern has been sent - if (OreoLED_PX4::_pattern_override != 0) { - // reset stage so patterns will be resent once override clears - last_stage = 0; - return; - } + sync_counter(); // syncronizes LEDs every 10 seconds - // slow rate from 50Hz to 10hz - counter++; - if (counter < 5) { - return; - } - counter = 0; + if (mode_firmware_update()) { return; } // don't go any further if the Pixhawk is in firmware update - // move forward one step - step++; - if (step >= 10) { - step = 0; - } + if (mode_init()) { return; } // don't go any further if the Pixhawk is initializing - // Pre-initialization pattern is all solid green - if (!initialization_done) { - set_rgb(OREOLED_ALL_INSTANCES, 0, OREOLED_BRIGHT, 0); - } + if (mode_failsafe_radio()) { return; } // don't go any further if the Pixhawk is is in radio failsafe - // initialising pattern - if (AP_Notify::flags.initialising) { - initialization_done = 1; // Record initialization has begun - last_stage = 1; // record stage + set_standard_colors(); // set the rear LED standard colors as described above - // exit so no other status modify this pattern - return; - } + if (mode_failsafe_batt()) { return; } // stop here if the battery is low. - // save trim and esc calibration pattern - if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) { - switch(step) { - case 0: - case 3: - case 6: - // red - set_rgb(OREOLED_INSTANCE_ALL, OREOLED_BRIGHT, 0, 0); - break; + if (_pattern_override) { return; } // stop here if in mavlink LED control override. - case 1: - case 4: - case 7: - // blue - set_rgb(OREOLED_INSTANCE_ALL, 0, 0, OREOLED_BRIGHT); - break; + if (mode_auto_flight()) { return; } // stop here if in an autopilot mode. - case 2: - case 5: - case 8: - // green on - set_rgb(OREOLED_INSTANCE_ALL, 0, OREOLED_BRIGHT, 0); - break; + mode_pilot_flight(); // stop here if in an pilot controlled mode. +} - case 9: - // all off - set_rgb(OREOLED_INSTANCE_ALL, 0, 0, 0); - break; - } - // record stage - last_stage = 2; - // exit so no other status modify this pattern - return; - } +// Slow the update rate from 50hz to 10hz +// Returns true if counting up +// Returns false and resets one counter hits 5 +bool OreoLED_PX4::slow_counter() +{ + static uint8_t update_counter; - // radio failsafe pattern: Alternate between front red/rear black and front black/rear red - if (AP_Notify::flags.failsafe_radio) { - switch(step) { - case 0: - case 1: - case 2: - case 3: - case 4: - // Front red/rear black - set_rgb(OREOLED_FRONTLEFT, OREOLED_BRIGHT, 0, 0); - set_rgb(OREOLED_FRONTRIGHT, OREOLED_BRIGHT, 0, 0); - set_rgb(OREOLED_BACKLEFT, 0, 0, 0); - set_rgb(OREOLED_BACKRIGHT, 0, 0, 0); - break; - case 5: - case 6: - case 7: - case 8: - case 9: - // Front black/rear red - set_rgb(OREOLED_FRONTLEFT, 0, 0, 0); - set_rgb(OREOLED_FRONTRIGHT, 0, 0, 0); - set_rgb(OREOLED_BACKLEFT, OREOLED_BRIGHT, 0, 0); - set_rgb(OREOLED_BACKRIGHT, OREOLED_BRIGHT, 0, 0); - break; - } - // record stage - last_stage = 3; - // exit so no other status modify this pattern - return; - } - - // send colours (later we will set macro if required) - if (last_stage < 10 && initialization_done) { - set_macro(OREOLED_INSTANCE_ALL, OREOLED_PARAM_MACRO_AUTOMOBILE); - last_stage = 10; - } else if (last_stage >= 10) { - static uint8_t previous_autopilot_mode = -1; - if (previous_autopilot_mode != AP_Notify::flags.autopilot_mode) { - - if (AP_Notify::flags.autopilot_mode) { - // autopilot flight modes start breathing macro - set_macro(OREOLED_INSTANCE_ALL, OREOLED_PARAM_MACRO_AUTOMOBILE); - set_macro(OREOLED_INSTANCE_ALL, OREOLED_PARAM_MACRO_BREATH); - } else { - // manual flight modes stop breathing -- solid color - set_macro(OREOLED_INSTANCE_ALL, OREOLED_PARAM_MACRO_AUTOMOBILE); - } - - // record we have processed this change - previous_autopilot_mode = AP_Notify::flags.autopilot_mode; - } - last_stage = 11; + update_counter++; + if (update_counter < 5) { + return 1; + } else { + update_counter = 0; + return 0; } } -// set_rgb - set color as a combination of red, green and blue values for one or all LEDs -void OreoLED_PX4::set_rgb(uint8_t instance, uint8_t red, uint8_t green, uint8_t blue) + +// Calls resyncing the LEDs every 10 seconds +// Always returns false, no action needed. +void OreoLED_PX4::sync_counter() { - // return immediately if no healty leds - if (!_overall_health) { - return; + static uint8_t counter = 80; + + counter++; + if (counter > 100) { + counter = 0; + send_sync(); + } +} + + +// Procedure for when Pixhawk is in FW update / bootloader +// Makes all LEDs go into color cycle mode +// Returns true if firmware update in progress. False if not +bool OreoLED_PX4::mode_firmware_update() +{ + if (AP_Notify::flags.firmware_update) { + set_macro(OREOLED_INSTANCE_ALL, OREOLED_PARAM_MACRO_COLOUR_CYCLE); + return true; + } else { + return false; + } +} + + +// Procedure for when Pixhawk is booting up and initializing +// Makes all LEDs rapidly strobe blue. Returns LED_INIT_STAGE +// 1 = Default, initialization has not yet begun +// 2 = Initialization flag found, initialization in progress +// 3 = Initialization flag no longer found, initialization complete +bool OreoLED_PX4::mode_init() +{ + static uint16_t stage = 1; + + // Pixhawk has not begun initializing yet. Strobe all blue + if ((!AP_Notify::flags.initialising) && ((stage == 1))) { + set_rgb(OREOLED_INSTANCE_ALL, OREOLED_PATTERN_STROBE, 0, 0, 255,0,0,0,PERIOD_SUPER,0); + + // Pixhawk has begun initializing + } else if ((AP_Notify::flags.initialising) && ((stage == 1))) { + stage = 2; + set_rgb(OREOLED_INSTANCE_ALL, OREOLED_PATTERN_STROBE, 0, 0, 255,0,0,0,PERIOD_SUPER,0); + + // Pixhawk still initializing + } else if ((AP_Notify::flags.initialising) && ((stage == 2))) { + set_rgb(OREOLED_INSTANCE_ALL, OREOLED_PATTERN_STROBE, 0, 0, 255,0,0,0,PERIOD_SUPER,0); + + // Pixhawk has completed initialization + } else if((!AP_Notify::flags.initialising) && ((stage == 2))) { + stage = 0; + set_rgb(OREOLED_INSTANCE_ALL, OREOLED_PATTERN_SOLID, 0, 0, 255); + + } else { stage = 0; } + + return stage; +} + + +// Procedure for when Pixhawk is in radio failsafe +// LEDs perform alternating Red X pattern +bool OreoLED_PX4::mode_failsafe_radio() +{ + if (AP_Notify::flags.failsafe_radio) { + set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SLOW,0); + set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SLOW,PO_ALTERNATE); + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SLOW,PO_ALTERNATE); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SLOW,0); + } + return AP_Notify::flags.failsafe_radio; +} + + +// Procedure to set standard rear LED colors in aviation theme +// Back LEDS White for normal, yellow for GPS not usable, purple for EKF bad] +// Returns true GPS or EKF problem, returns false if all ok +bool OreoLED_PX4::set_standard_colors() +{ + if (!(AP_Notify::flags.gps_fusion)) { + _rear_color_r = 255; + _rear_color_g = 50; + _rear_color_b = 0; + return true; + + } else if (AP_Notify::flags.ekf_bad){ + _rear_color_r = 255; + _rear_color_g = 0; + _rear_color_b = 255; + return true; + + } else { + _rear_color_r = 255; + _rear_color_g = 255; + _rear_color_b = 255; + return false; + } +} + + +// Procedure to set low battery LED output +// Colors standard +// Fast strobe alternating front/back +bool OreoLED_PX4::mode_failsafe_batt() +{ + if (AP_Notify::flags.failsafe_battery){ + + switch (_oreo_theme) { + case OreoLED_Aircraft: + set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_FAST,0); + set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 0, 255, 0,0,0,0,PERIOD_FAST,0); + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_FAST,PO_ALTERNATE); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_FAST,PO_ALTERNATE); + break; + + case OreoLED_Automobile: + set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_FAST,0); + set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_FAST,0); + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_FAST,PO_ALTERNATE); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_FAST,PO_ALTERNATE); + break; + + default: + set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_FAST,0); + set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_FAST,0); + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_FAST,PO_ALTERNATE); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_FAST,PO_ALTERNATE); + break; + } + } + return AP_Notify::flags.failsafe_battery; +} + + +// Procedure for when Pixhawk is in an autopilot mode +// Makes all LEDs strobe super fast using standard colors +bool OreoLED_PX4::mode_auto_flight() +{ + switch (_oreo_theme) { + + case OreoLED_Aircraft: + set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SUPER,0); + set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 0, 255, 0,0,0,0,PERIOD_SUPER,0); + if ((AP_Notify::flags.pre_arm_check && AP_Notify::flags.pre_arm_gps_check) || AP_Notify::flags.armed) { + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_SUPER,PO_ALTERNATE); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_SUPER,PO_ALTERNATE); + } else { + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_SOLID, _rear_color_r, _rear_color_g, _rear_color_b); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_SOLID, _rear_color_r, _rear_color_g, _rear_color_b); + } + break; + + case OreoLED_Automobile: + set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_SUPER,0); + set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_SUPER,0); + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SUPER,PO_ALTERNATE); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SUPER,PO_ALTERNATE); + break; + + default: + set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_SUPER,0); + set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_SUPER,0); + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SUPER,PO_ALTERNATE); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SUPER,PO_ALTERNATE); + break; + } + + return AP_Notify::flags.autopilot_mode; +} + + +// Procedure for when Pixhawk is in a pilot controlled mode +// All LEDs use standard pattern and colors +bool OreoLED_PX4::mode_pilot_flight() +{ + switch (_oreo_theme) { + + case OreoLED_Aircraft: + set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_SOLID, 255, 0, 0); + set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_SOLID, 0, 255, 0); + if ((AP_Notify::flags.pre_arm_check && AP_Notify::flags.pre_arm_gps_check) || AP_Notify::flags.armed) { + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_FAST,0); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_FAST,PO_ALTERNATE); + } else { + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_SOLID, _rear_color_r, _rear_color_g, _rear_color_b); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_SOLID, _rear_color_r, _rear_color_g, _rear_color_b); + } + break; + + case OreoLED_Automobile: + set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_SOLID, 255, 255, 255); + set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_SOLID, 255, 255, 255); + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_SOLID, 255, 0, 0); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_SOLID, 255, 0, 0); + break; + + default: + set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_SOLID, 255, 255, 255); + set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_SOLID, 255, 255, 255); + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_SOLID, 255, 0, 0); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_SOLID, 255, 0, 0); + break; } + return true; +} + + +// set_rgb - Solid color settings only +void OreoLED_PX4::set_rgb(uint8_t instance, uint8_t red, uint8_t green, uint8_t blue) +{ + set_rgb(instance, OREOLED_PATTERN_SOLID, red, green, blue); +} + + +// set_rgb - Set a color and selected pattern. +void OreoLED_PX4::set_rgb(uint8_t instance, oreoled_pattern pattern, uint8_t red, uint8_t green, uint8_t blue) +{ // get semaphore _state_desired_semaphore = true; @@ -248,21 +361,15 @@ void OreoLED_PX4::set_rgb(uint8_t instance, uint8_t red, uint8_t green, uint8_t if (instance == OREOLED_INSTANCE_ALL) { // store desired rgb for all LEDs for (uint8_t i=0; i> 8; + cmd.buff[15] = (_state_desired[i].period & 0x00FF); + cmd.buff[16] = OREOLED_PARAM_PHASEOFFSET; + cmd.buff[17] = (_state_desired[i].phase_offset & 0xFF00) >> 8; + cmd.buff[18] = (_state_desired[i].phase_offset & 0x00FF); + cmd.num_bytes = 19; + ioctl(_oreoled_fd, OREOLED_SEND_BYTES, (unsigned long)&cmd); + } + break; case OREOLED_MODE_SYNC: { ioctl(_oreoled_fd, OREOLED_FORCE_SYNC, 0); } break; - } + default: + break; + }; // save state change _state_sent[i] = _state_desired[i]; } @@ -376,7 +560,8 @@ void OreoLED_PX4::update_timer(void) _send_required = false; } -// handle a LED_CONTROL message + +// Handle an LED_CONTROL mavlink message void OreoLED_PX4::handle_led_control(mavlink_message_t *msg) { // exit immediately if unhealthy @@ -396,17 +581,142 @@ void OreoLED_PX4::handle_led_control(mavlink_message_t *msg) // if pattern is OFF, we clear pattern override so normal lighting should resume if (packet.pattern == LED_CONTROL_PATTERN_OFF) { _pattern_override = 0; + clear_state(); return; } - // custom patterns not implemented if (packet.pattern == LED_CONTROL_PATTERN_CUSTOM) { - return; - } + // Here we handle two different "sub commands", + // depending on the bytes in the first CUSTOM_HEADER_LENGTH + // of the custom pattern byte buffer - // other patterns sent as macro - set_macro(packet.instance, (oreoled_macro)packet.pattern); + // Return if we don't have at least CUSTOM_HEADER_LENGTH bytes + if (packet.custom_len < CUSTOM_HEADER_LENGTH) { + return; + } + + // check for the RGB0 sub-command + if (memcmp(packet.custom_bytes, "RGB0", CUSTOM_HEADER_LENGTH) == 0) { + // check to make sure the total length matches the length of the RGB0 command + data values + if (packet.custom_len != CUSTOM_HEADER_LENGTH + 4) { + return; + } + + // check for valid pattern id + if (packet.custom_bytes[CUSTOM_HEADER_LENGTH] >= OREOLED_PATTERN_ENUM_COUNT) { + return; + } + + // convert the first byte after the command to a oreoled_pattern + oreoled_pattern pattern = (oreoled_pattern)packet.custom_bytes[CUSTOM_HEADER_LENGTH]; + + // call the set_rgb function, using the rest of the bytes as the RGB values + set_rgb(packet.instance, pattern, packet.custom_bytes[CUSTOM_HEADER_LENGTH + 1], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 2], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 3]); + + } else if (memcmp(packet.custom_bytes, "RGB1", CUSTOM_HEADER_LENGTH) == 0) { // check for the RGB1 sub-command + + // check to make sure the total length matches the length of the RGB1 command + data values + if (packet.custom_len != CUSTOM_HEADER_LENGTH + 11) { + return; + } + + // check for valid pattern id + if (packet.custom_bytes[CUSTOM_HEADER_LENGTH] >= OREOLED_PATTERN_ENUM_COUNT) { + return; + } + + // convert the first byte after the command to a oreoled_pattern + oreoled_pattern pattern = (oreoled_pattern)packet.custom_bytes[CUSTOM_HEADER_LENGTH]; + + // uint16_t values are stored in custom_bytes in little endian order + // assume the flight controller is little endian when decoding values + uint16_t period = + ((0x00FF & (uint16_t)packet.custom_bytes[CUSTOM_HEADER_LENGTH + 7]) << 8) | + (0x00FF & (uint16_t)packet.custom_bytes[CUSTOM_HEADER_LENGTH + 8]); + uint16_t phase_offset = + ((0x00FF & (uint16_t)packet.custom_bytes[CUSTOM_HEADER_LENGTH + 9]) << 8) | + (0x00FF & (uint16_t)packet.custom_bytes[CUSTOM_HEADER_LENGTH + 10]); + + // call the set_rgb function, using the rest of the bytes as the RGB values + set_rgb(packet.instance, pattern, packet.custom_bytes[CUSTOM_HEADER_LENGTH + 1], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 2], + packet.custom_bytes[CUSTOM_HEADER_LENGTH + 3], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 4], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 5], + packet.custom_bytes[CUSTOM_HEADER_LENGTH + 6], period, phase_offset); + } else if (memcmp(packet.custom_bytes, "SYNC", CUSTOM_HEADER_LENGTH) == 0) { // check for the SYNC sub-command + // check to make sure the total length matches the length of the SYN0 command + data values + if (packet.custom_len != CUSTOM_HEADER_LENGTH + 0) { + return; + } + send_sync(); + } else { // unrecognized command + return; + } + } else { + // other patterns sent as macro + set_macro(packet.instance, (oreoled_macro)packet.pattern); + } _pattern_override = packet.pattern; } +OreoLED_PX4::oreo_state::oreo_state() { + clear_state(); +} + +void OreoLED_PX4::oreo_state::clear_state() { + mode = OREOLED_MODE_NONE; + pattern = OREOLED_PATTERN_OFF; + macro = OREOLED_PARAM_MACRO_RESET; + red = 0; + green = 0; + blue = 0; + amplitude_red = 0; + amplitude_green = 0; + amplitude_blue = 0; + period = 0; + repeat = 0; + phase_offset = 0; +} + +void OreoLED_PX4::oreo_state::send_sync() { + clear_state(); + mode = OREOLED_MODE_SYNC; +} + +void OreoLED_PX4::oreo_state::set_macro(oreoled_macro new_macro) { + clear_state(); + mode = OREOLED_MODE_MACRO; + macro = new_macro; +} + +void OreoLED_PX4::oreo_state::set_rgb(enum oreoled_pattern new_pattern, uint8_t new_red, uint8_t new_green, uint8_t new_blue) { + clear_state(); + mode = OREOLED_MODE_RGB; + pattern = new_pattern; + red = new_red; + green = new_green; + blue = new_blue; +} + +void OreoLED_PX4::oreo_state::set_rgb(enum oreoled_pattern new_pattern, uint8_t new_red, uint8_t new_green, + uint8_t new_blue, uint8_t new_amplitude_red, uint8_t new_amplitude_green, uint8_t new_amplitude_blue, + uint16_t new_period, uint16_t new_phase_offset) { + clear_state(); + mode = OREOLED_MODE_RGB_EXTENDED; + pattern = new_pattern; + red = new_red; + green = new_green; + blue = new_blue; + amplitude_red = new_amplitude_red; + amplitude_green = new_amplitude_green; + amplitude_blue = new_amplitude_blue; + period = new_period; + phase_offset = new_phase_offset; +} + +bool OreoLED_PX4::oreo_state::operator==(const OreoLED_PX4::oreo_state &os) { + return ((os.mode==mode) && (os.pattern==pattern) && (os.macro==macro) && (os.red==red) && (os.green==green) && (os.blue==blue) + && (os.amplitude_red==amplitude_red) && (os.amplitude_green==amplitude_green) && (os.amplitude_blue==amplitude_blue) + && (os.period==period) && (os.repeat==repeat) && (os.phase_offset==phase_offset)); +} + #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 + diff --git a/libraries/AP_Notify/OreoLED_PX4.h b/libraries/AP_Notify/OreoLED_PX4.h index 725a11c0d5..59d5d6bc33 100644 --- a/libraries/AP_Notify/OreoLED_PX4.h +++ b/libraries/AP_Notify/OreoLED_PX4.h @@ -26,11 +26,13 @@ #define OREOLED_INSTANCE_ALL 0xff // instance number to indicate all LEDs (used for set_rgb and set_macro) #define OREOLED_BRIGHT 0xff // maximum brightness when flying (disconnected from usb) +#define CUSTOM_HEADER_LENGTH 4 // number of bytes in the custom LED buffer that are used to identify the command + class OreoLED_PX4 : public NotifyDevice { public: // constuctor - OreoLED_PX4(); + OreoLED_PX4(uint8_t theme); // init - initialised the device bool init(void); @@ -49,23 +51,53 @@ private: // update_timer - called by scheduler and updates PX4 driver with commands void update_timer(void); - // set_rgb - set color as a combination of red, green and blue values for one or all LEDs + // set_rgb - set color as a combination of red, green and blue values for one or all LEDs, pattern defaults to solid color void set_rgb(uint8_t instance, uint8_t red, uint8_t green, uint8_t blue); + // set_rgb - set color as a combination of red, green and blue values for one or all LEDs, using the specified pattern + void set_rgb(uint8_t instance, enum oreoled_pattern pattern, uint8_t red, uint8_t green, uint8_t blue); + + // set_rgb - set color as a combination of red, green and blue values for one or all LEDs, using the specified pattern and other parameters + void set_rgb(uint8_t instance, oreoled_pattern pattern, uint8_t red, uint8_t green, uint8_t blue, + uint8_t amplitude_red, uint8_t amplitude_green, uint8_t amplitude_blue, + uint16_t period, uint16_t phase_offset); + // set_macro - set macro for one or all LEDs void set_macro(uint8_t instance, enum oreoled_macro macro); - // send_sync - force a syncronisation of the LEDs - void send_sync(void); + // send_sync - force a syncronisation of the all LED's + void send_sync(); + + // functions to set LEDs to specific patterns. These functions return true if no further updates should be made to LEDs this iteration + bool slow_counter(void); + void sync_counter(void); + bool mode_firmware_update(void); + bool mode_init(void); + bool mode_failsafe_radio(void); + bool set_standard_colors(void); + bool mode_failsafe_batt(void); + bool mode_auto_flight(void); + bool mode_pilot_flight(void); + + // Clear the desired state + void clear_state(void); // oreo led modes (pattern, macro or rgb) enum oreoled_mode { - OREOLED_MODE_PATTERN, + OREOLED_MODE_NONE, OREOLED_MODE_MACRO, OREOLED_MODE_RGB, + OREOLED_MODE_RGB_EXTENDED, OREOLED_MODE_SYNC }; + // Oreo LED modes + enum Oreo_LED_Theme { + OreoLED_Disabled = 0, + OreoLED_Aircraft = 1, + OreoLED_Automobile = 2, + }; + // oreo_state structure holds possible state of an led struct oreo_state { enum oreoled_mode mode; @@ -74,11 +106,28 @@ private: uint8_t red; uint8_t green; uint8_t blue; + uint8_t amplitude_red; + uint8_t amplitude_green; + uint8_t amplitude_blue; + uint16_t period; + int8_t repeat; + uint16_t phase_offset; - // operator== - inline bool operator==(const oreo_state &os) { - return ((os.mode==mode) && (os.pattern==pattern) && (os.macro==macro) && (os.red==red) && (os.green==green) && (os.blue==blue)); - } + oreo_state(); + + void clear_state(); + + void send_sync(); + + void set_macro(oreoled_macro new_macro); + + void set_rgb(enum oreoled_pattern new_pattern, uint8_t new_red, uint8_t new_green, uint8_t new_blue); + + void set_rgb(enum oreoled_pattern new_pattern, uint8_t new_red, uint8_t new_green, + uint8_t new_blue, uint8_t new_amplitude_red, uint8_t new_amplitude_green, uint8_t new_amplitude_blue, + uint16_t new_period, uint16_t new_phase_offset); + + bool operator==(const oreo_state &os); }; // private members @@ -89,6 +138,10 @@ private: oreo_state _state_desired[OREOLED_NUM_LEDS]; // desired state oreo_state _state_sent[OREOLED_NUM_LEDS]; // last state sent to led uint8_t _pattern_override; // holds last processed pattern override, 0 if we are not overriding a pattern + uint8_t _oreo_theme; // theme (1=AirCraft, 2=Ground Vehicle) + uint8_t _rear_color_r = 255; // the rear LED red value + uint8_t _rear_color_g = 255; // the rear LED green value + uint8_t _rear_color_b = 255; // the rear LED blue value }; #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4