100KM

U4eake's showleds, arming and low battery warning leds

Following up on the success of my relay low battery warning leds I have expanded that code to include a complete lightshow and to be compatible with the newest ACM codes (2.1.1) and the APM1 hardware.  APM2 hardware MAY work, but I don't have APM2 at this time, so no guarantees.

Credits to Bill Sanford and Max Levine whose work has been an inspiration and a basis for these leds.

 

1) Software side :

By using the agmatthews userhooks it is now much simpler to upgrade to a new firmware without having to change the code each time.  You just have to copy 2 files and change setting in the APM_config.h.  The needed files AND my config.h file (for version 2.1.1r5) are in the following file : U4eake_showleds.zip 

Updated files for ArduCopter 2.5 (compile with arduino1.0 relax patch) : u4-showleds.zip  These still work for ArduCopter 2.5.3.

Update files for ArduCopter 2.6 : Showleds_ACM2.6.zip  Because motors were put in a library, every motor_armed in the showleds code had to be modified to motors.armed().  Also you have to put

#define COPTER_LEDS 0

into your APM_Config.h file to disable the new copterleds, which are enabled by default.


You need to copy the usercode.pde and the usercode.h files into your arducopter directory (overwriting the files already there) and add the settings below to your APM_config.h.  DO NOT just copy my APM_config.h, it is only included as an example to clarify the settings.

#define MOTOR_LEDS 0                                   // 0 = off, 1 = on
#define SHOW_LEDS 1                                    // set to 1 to enable AN7-AN15 controlled led lightshow
#define RELAY_LEDS 0                                    // set to 1 to use the old relay led low battery warning

#define COPTER_LEDS 0

#define BATTERY_EVENT ENABLED

#define LOW_VOLTAGE 9.9
#define MID_VOLTAGE 10.6
#define VOLT_DIV_RATIO 3.33

#define INPUT_VOLTAGE 5.26

Also uncomment the 50hz agmatthews userhook and the mediumloop userhook (remove the // in front of it) so it looks like this :

#define USERHOOK_50HZLOOP userhook_50Hz();

#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();

 

The voltages can be adjusted to match your desires.  If you have a 4S battery for example, you should put higher values, like 13.2V for the LOW_VOLTAGE.  INPUT_VOLTAGE is the voltage that goes into the APM (usually the voltage from you ubec).  VOLT_DIV_RATIO is an calibration value that you can adjust so that your reported battery voltage corresponds to the real battery voltage.

 

After compiling the code with arduino and uploading it to the APM board, you also need to set battery monitoring to option 3 : battery volts

3689416774?profile=original

 

That's it for the software modifications if you have a hexa or are happy with the patterns as they are !  On a quad or octo you may want to adjust the patterns a bit, but I'll explain that further down this blog (see Adjusting Patterns).

 

The code and blink patterns are optimised for use on my hexa (6 leds strips on the arms + 2 spare ones).  

The leds will go on when motors are armed and out when disarmed.  During flight, different flashing patterns can be selected with Ch7. The other use of Ch7 is not affected, so you can still use it for simple or recording waypoints. The leds will flash with the last pattern in this file when ch7 is high (recording waypoint or simple)
When battery gets below the first threshold (MID_VOLTAGE = 10.6V), the ledshow is overridden and the leds will flash with following pattern : led 1,3,5 on/off and led 2,4,6 off/on. So if the first ones are off, the others are on and vice versa.
When second threshold (LOW_VOLTAGE)is reached (9.9V) the leds will flash quicker.

 

2) Hardware :

The code uses the AN8 tot AN15 analog ports (portK) on the oilpan of APM1.  In the picture you can see them labelled  AN0-AN15.  AN8 to AN15 are the ones on the right (4x2 pins)

3689439386?profile=original

 

You can either connect 1 led directly to each port (the ports can supply 40mA current) or you can use a ULN2803 darlington transistor chip to drive the leds according to the following shedule (thx to Max Levine for this schematic) :

3689414934?profile=original

This shedule is for a quad and uses a ULN2003.  The ULN2803 has 8 ports instead of 7 and if I remember correctly it can supply a little more currrent (500mA per port).  A typical hobbyking 12V ledstrip of 1m (60 leds) consumes about 300mA, so normally you should be able to hang 1m of ledstrip on each of the 8 ports.  That should be plenty !!

You can get the ULN2803 in your local electronics shop or online here : Canadadrones - ULN2803

The darlington chip lets each input control the output directly across on the other side of the chip.  So nr1 in the picture above, controls output 16,   Nr 2 controls output 15 and so on.  If you look at the pinout picture of the oilpan, you'll see that nr 1 is connected to AN8 and nr2 is connected to AN10, so AN8 in the pictures above controls the red leds, and AN10 controls the green leds.  Get it ? ;-)

 

After the brainsurgery it will look something like this :

ULN2803.jpg

Now you just have to put a heatshrink over the ULN2803 chip and tuck all the wires away nicely.

CAREFULL : in the picture above I had soldered the wires to the wrong ports (side of the compass).  That's what you get when working on your copter till 4 in the morning...  It took me a day to figure out why my leds didn't work as expected.  So you have to solder your wires on the OTHER SIDE (side of the usb connector).  I didn't have a picture of when I soldered it right.

Finally you'll also have to set up your board for battery monitoring (option 3 : total voltage) by soldering the voltage dividers and connecting battery voltage to AN0, which is explained in the wiki here :  Voltage sensors on APM1

 

3) Adjusting the blinking patterns

First you'll have to understand a little about bitmath in arduino : http://www.arduino.cc/playground/Code/BitMath

To save processor cycles I've opted to drive the portK of the atmega1280 using direct portmanipulation. 

If the above all sounds too complicated : here's the simple explanation.

In the usercode you'll often find something like PORTK = B00101010;  This instruction actually turns leds on and off.  0 means off and 1 means on.  Each number turns 1 port off or on, and the first number after B turns port AN15 on. So the ports are B-AN15-AN14-AN13-AN12-AN11-AN10-AN9-AN8 of shorter written : BAN15...AN8

If you would now like to turn the red leds on and the others off in the picture above, this would be PORTK = B00000001; (turns AN8 on, and AN8 is connect to nr1 on the darlington, which turns on red leds)

If you wanna turn the 2 blue ledstrips on and the others off it would be PORTK = B01010000; (ports AN12 and AN14 on, which are connect through the darlington to blue leds)

All 4 strips on would be PortK = B01010101;

Ofcourse for hexa's and octo's this depends on which AN port you have soldered to which nr on the ULN2803.

 

Now we can turn leds on and off, but they still don't blink or "rotate".  For the blinking a counter is used and depending on the counter, leds are switched on and off.  The counter runs in the 10hz or 50hz loop, so you can time the blinking.  If counter =5 when in the 10hz loop will correspond to half a second.  

For "rotating" arms I use the bitmath again, more precisely the bitshift operators << and >>.  These simple shift all bit a few places.  so 00000001 << 1 becomes 00000010.  The 1 shifts 1 place.  By now you understood that this is an easy way to make the leds "rotate".  Just shift the B00000001 1 place each loop cycle and your 'on' leds will shift an arm each cycle, making them look asif they rotate.

 

4) TO DO

On the 8 port ULN2803 there are still 2 ports left unused on my hexa.  I'm planning to use those to replicate the gps status light and maybe a failsafe warning light.  I've also thought about using them to drive leds in the form of a smiley :-)  Other ideas are always welcome !

 

I'm using the relay now to ignite fireworks on the hexa.  But that's a story for another blog ;-)

 

That's about all I can think of telling you, and by now my fingers are tired and my brain is mushy.

I hope you enjoy these leds and don't scare too many people with your new UFO ;-)  Feel free to ask questions in the comments below if something is not clear.

Ow, and ofcourse I assume that you know what you're doing if you solder on your board.  I take NO responsability if something goes wrong and you break it !  Do this at your own risk ! 

 

Regards,

U4

 

E-mail me when people leave their comments –

You need to be a member of diydrones to add comments!

Join diydrones

Comments

  • does that do similar function?

    http://www.goodluckbuy.com/apm2-5-2-5-2-apm2-6-diy-led-light-switch...

     

     

  • Hi Rich,

    I was already disabling MOUNT, CAMERA, OPTFLOW and AC_FENCE when I tried it the first time.  I had to include GPS_PROTOCOL which took the sketch size down from 247kb to 238kb and now I'm able to upload without any issues...

  • Andre, it is possible that your APM doesn't have enough space to fit the binary. Try uncommenting some of the space saving code at the bottom of APM_Config.h and see how you go flashing a smaller binary. Good luck :)

  • Has anyone been able to upload the new 3.1 firmware?  I get a "verification error / content mismatch" error after the upload process and the MP wont connect to the APM, but when I upload the firmware to the APM via the MP it works...

  • Is there a way to toggle individual pin outs, rather than using PORTK which sets all of the output pins? I want to run LEDs of Pins 8 and 9 without interfere with other outputs on 10 and 11.

  • Done...

    APM2 LED Show

  • Welcome!

    For real though, I'm a full on hack when it comes to programming... I make it up as I go LOL. In college just I did the minimum CE requirements to get my BSAE; entry level C, plus various excercises in in other courses using Labview, Matlab, and Mathematica. I've given countless test flight hours to the community over the past year, but no useful code until now. If it weren't for ppl like you, Rob, Randy, and others (too many to name) who selflessly share their knowledge here, I would probably have nothing but crash reports to share. :)

    I completed the finishing touches on the code today, and I am working on forming a decent thread (need pics, and have to wait for tonight to get decent video of the LEDs). After I post, I'll report back here with a link. Likewise, in my thread I will include a link to your thread.

  • 100KM

    Hi Kevin,

    Not bad at all for "not being a programmer" ;-)

    If you share the final code I will also use it on my new AC3.01 quad, and I'm sure many other will too.  If you make a new blog about it I'll modify my post with a link to yours

    Thx for the port !

  • OK, I got it working now and so far I noticed no difference in loop performance! It's not the code above... I had to make lots of changes to that, and I moved some of the declarations to "user_variables". The things that really got me were a) different formatting for case labels in the GPS status switch, and b) the APM Arduino IDE does not understand "BXXXXXXXX", instead it wants to see "0bXXXXXXXX". Anyhow, I'll make a new post and share once I get some video.

    Thanks again for your encouragement,

    Kev

    HelpWikipedia: Help is any form of assistance.

  • Weird how my reply's take forever to show up on my PC (hence my double posting above), but anyways...

    I did get my code to compile, but I had to declare a bunch of variables that you did not (I guess because the code is all in the loop). I also was frustrated that it did not compile with byte format used... so I had to convert every single "BXXXXXXXX" to decimal format. I am not sure why, but maybe it has to do with the new HAL stuff that is somewhat new.

    Am I missing something? Is there a library that used to be there that I should add back in?

    I'd like to stick with byte format to keep my code easy to read and edit, but I think it may work as is. Here's a copy if you want to check it out:

    /*KevinB's APM2 FPV Quadcopter LED Show, GPS, and Voltage status display

    This usercode, written by KevinB, uses the agmatthews userhooks for an LED show,
    driven by the PORTF of the APM2.  It is based on work of u4eake, R_Lefebvre
    and Max Levine.

    Connect your LEDs like this:
         AN7   AN6   AN5   AN4   AN3                -APM2 Plug ID
         Fr/L  Fr/R  Rr/R  Rr/L  GPS                -LED Arm Location (does NOT follow Arducopter esc numbering convention!!!)
      B  x     x     x     x     x      xxx         -Corresponding bits of PORTF (Atmega2560, the first 3 bits are reserved for sensors)

    To enable, you can add these lines in your APM_config.h file:
      #define SHOW_LEDS             1               -Only SHOW_LEDS is not accessible through the Planner
      #define BATTERY_EVENT            ENABLED         -The rest of these items can be set in the Planner if you wish
      #define LOW_VOLTAGE            9.9
      #define MID_VOLTAGE            10.6
      #define VOLT_DIV_RATIO    3.33
      #define INPUT_VOLTAGE            5.26
      #define COPTER_LEDS            0

    The GPS LED always works as follows:
      STROBE   -3d lock / home is set                -double flash / 2sec period
      ON       -3d lock / no home set
      TOGGLE   -3D lock                              -1sec on / 1sec off
      OFF      -no GPS data

    The leg led show will go on when motors are armed and out when disarmed
    During flight, different flashing patterns can be selected with Ch8.  
    The other use of Ch8 is not affected, so you can still use it for
    other functions.  The leg leds will flash with the last pattern
    in this file when ch8 is high (recording waypoint or simpl)
    When battery gets below the first threshold (MID_VOLTAGE = 10.6V), the ledshow
    is overridden and the legs will flash arms left to right twice per second.
    When second threshold is reached (LOW_VOLTAGE = 9.9V) the legs will flash quicker.

    If you want to modify this code yourself, make sure you understand arduino bitmath
    http://www.arduino.cc/playground/Code/BitMath
    especially the part : "common problems solved"
    Unfortunately, AC3 did not support byte formatting (or at least I couldn't get it to work).
    This means the patterns are hard to decipher, and bitmath is required to edit the code.
    IE, instead of turning GPS LED on with "B00001000", we must use non-intuitive
    decimal "8".

    For more information on u4eake's showleds which this code is based on, check out this page:
    http://diydrones.com/profiles/blogs/u4eake-s-showleds-arming-and-lo...

    Enjoy !
    */

    void userhook_init(void) {
        // put your initialisation code here
            #if SHOW_LEDS == 1
               DDRF = 248;                                // set the ports, reserving AN0-2 for A, V, and RSSI sensors
            #endif
    }

    void userhook_MediumLoop(void) {                           //This loop runs at 10hz
    #if SHOW_LEDS == 1

               int legleds = 240;                            // used to make sure light show does not switch GPS LED or sensors
               int gpsled_counter = 0;
               int gpsbits = 0;
               int legpattern = 0;
               int led_counter = 0;
               int legbits = 0;

      //Set GPS LED on or off
      switch (g_gps->status()) {                               //check GPS status
        case (2):                                              //GPS is locked
            if(ap.home_is_set) {                               //Home is set, strobe flash GPS LED (2 flash 2sec period)
                  gpsled_counter++ ;      
                  if ( 0 < gpsled_counter && gpsled_counter < 2 ) gpsbits = 8;
                  else if ( 1 < gpsled_counter && gpsled_counter < 6 ) gpsbits =  0;
                  else if ( 5 < gpsled_counter && gpsled_counter < 7 ) gpsbits =  8;             
                  else if (gpsled_counter > 6 && gpsled_counter < 21) gpsbits =  0;
                  else gpsled_counter = 0;
            }
            else {                                             //Home not set, GPS LED on
                gpsbits = 8;
            }
            break;
        case (1):                                              //GPS receiving but not locked, toggle GPS LED every 1 second
            if (g_gps->valid_read == true) {                   
              gpsled_counter++ ;
              if ( 0 < gpsled_counter && gpsled_counter < 11 )
                {
                  gpsbits = 8;  
                }
              else if (10 < gpsled_counter && gpsled_counter < 21)
                {
                  gpsbits = 0;  
                }
              else gpsled_counter = 0;  
              g_gps->valid_read = false;
            }
            break;
        default:                                               //No GPS data, turn GPS LED off
            gpsbits = 0;
            break;
      }

      //Set Leg LEDs
      if (motors.armed()) {                                    //motors armed, start the leg LED show   
        if  (battery_voltage1 > MID_VOLTAGE) {                 //voltage is OK, run the normal light show (ch8 options)
          if      (g.rc_8.control_in < 213) {                                                       //throttle varied boomerang - ch8 low
                 static int led_speed = 0;
                 static int led_limiter = 0;
                 led_speed = 10 - (g.rc_3.control_in / 50);
                 led_speed = max(led_speed, 1);
                 led_limiter = led_speed * 3;
                 legpattern =  51 << (led_counter / led_speed);
                 led_counter++;        
                 if (led_counter > led_limiter) led_counter = 0;
          }
          else if (212 < g.rc_8.control_in && g.rc_8.control_in < 236) legpattern =  160;      // legs 1 & 3 on           
          else if (235 < g.rc_8.control_in && g.rc_8.control_in < 265) legpattern =  80;      // legs 2 & 4 on
          else if (264 < g.rc_8.control_in && g.rc_8.control_in < 293) legpattern =  0;      // all legs off
          else if (292 < g.rc_8.control_in && g.rc_8.control_in < 320) {                             // all legs strobe (2 flash 1.5sec period)
                  led_counter++ ;      
                  if ( 0 < led_counter && led_counter < 2 ) legpattern = 240;
                  else if ( 1 < led_counter && led_counter < 6 ) legpattern =  0;
                  else if ( 5 < led_counter && led_counter < 7 ) legpattern =  240;             
                  else if (led_counter > 6 && led_counter < 16) legpattern =  0;
                  else led_counter = 0;
          }
          else if (319 < g.rc_8.control_in && g.rc_8.control_in < 347) {                             // toggle front to back                                          
                  if (led_counter > 0 && led_counter < 4) legpattern =  192;                    
                  else if (led_counter > 3 && led_counter < 6) legpattern =  48;
                  else led_counter = 0;           
                  led_counter++ ;      
          }        
          else if (346 < g.rc_8.control_in && g.rc_8.control_in < 375)  {                            // rotating legs
                  if (led_counter > 7) led_counter = 0;
                  legpattern =  16 << (led_counter / 2);
                  led_counter++ ;      
          }
          else if (374 < g.rc_8.control_in && g.rc_8.control_in < 401)                               // rotating boomerang
                 {
                  if (led_counter > 7) led_counter = 0;   
                  legpattern =  50 << (led_counter / 2);
                  led_counter++ ;      
          }  
          else if (400 < g.rc_8.control_in && g.rc_8.control_in < 427)                               //front strobe, rear on (2 quick flash, 1sec period)
                 {
                  led_counter++ ;          
                  if ( 0 < led_counter && led_counter < 2 ) legpattern = 240;
                  else if ( 1 < led_counter && led_counter < 6 ) legpattern =  48;
                  else if ( 5 < led_counter && led_counter < 7 ) legpattern =  240;             
                  else if (led_counter > 6 && led_counter < 11) legpattern =  48;
                  else led_counter = 0;           
          }  
          else if (426 < g.rc_8.control_in && g.rc_8.control_in < 468) {                             //front on, rear strobe (2 quick flash, 1sec period)
                  led_counter++ ;          
                  if ( 0 < led_counter && led_counter < 2 ) legpattern = 240;
                  else if ( 1 < led_counter && led_counter < 6 ) legpattern =  192;
                  else if ( 5 < led_counter && led_counter < 7 ) legpattern =  240;             
                  else if (led_counter > 6 && led_counter < 11) legpattern =  192;
                  else led_counter = 0;   
          }   
          else if (467 < g.rc_8.control_in && g.rc_8.control_in < 535) {                            //all legs on
                  legpattern =  240;                                                              
          }
          else if (534 < g.rc_8.control_in && g.rc_8.control_in < 600) {                            //3 legs rotating
                  if (led_counter > 7) led_counter = 0;   
                  legpattern =  118 << (led_counter / 2);
                  led_counter++ ;      
          }
          else if (599 < g.rc_8.control_in && g.rc_8.control_in < 633) {                            //toggling long line
                  if (led_counter > 7) led_counter = 0;   
                  legpattern =  85 << (led_counter / 2);
                  led_counter++ ;      
          }
          else if (632 < g.rc_8.control_in && g.rc_8.control_in < 663) {                            //knight rider
                  if (led_counter > 11) led_counter = 0;
                  if (led_counter < 8) legpattern =  16 << (led_counter / 2);
                  else legpattern =  64 >> ((led_counter - 8) / 2);  
                  led_counter++ ;      
          }
          else if (662 < g.rc_8.control_in && g.rc_8.control_in < 693) {                            //throttle varied rotating line
                 static int led_speed = 0;
                 static int led_limiter = 0;
                 led_speed = 10 - (g.rc_3.control_in / 50);
                 led_speed = max(led_speed, 1);
                 led_limiter = led_speed * 3;
                 legpattern =  17 << (led_counter / led_speed);
                 led_counter++;        
                 if (led_counter > led_limiter) led_counter = 0;
          }
          else if (692 < g.rc_8.control_in && g.rc_8.control_in < 723) {                           //fast flash all legs
                  if (led_counter == 1) legpattern =  240;
                  else {
                        legpattern =  0;
                        led_counter = 0;
                  }
                  led_counter++ ;      
          }
          else if (722 < g.rc_8.control_in && g.rc_8.control_in < 750) {                           //fast toggle front to back
                  if (led_counter == 1) legpattern =  48;
                  else {
                        legpattern =  192;
                        led_counter = 0;
                  }
                  led_counter++ ;      
          }
          else if (749 < g.rc_8.control_in && g.rc_8.control_in < 800) {                           //front on, fast flashing rear
                  if (led_counter == 1) legpattern =  240;
                  else {
                        legpattern =  192;
                        led_counter = 0;
                  }
                  led_counter++ ;      
          }

          else {                                                                                   //fast flashing front, rear on - ch8 high
                  if (led_counter == 1) legpattern =  240;
                  else {
                        legpattern =  48;
                        led_counter = 0;
                  }
                  led_counter++ ;      
          }
          legbits = legpattern & legleds;                                                          //do not allow legpattern to switch sensors or GPS LED
          PORTF = legbits | gpsbits;                                                               //Finally ready, turn the leg and GPS lights on or off
        }                                                       //End of the normal light show (ch8 options)=====================================================

        else if (battery_voltage1 < LOW_VOLTAGE) {                                                 //Low Voltage Override, flash left to right 5x per sec          
            led_counter++;
            if ( 0 < led_counter && led_counter < 3 ) {
                PORTF = gpsbits | 160;
            }
            else if (2 < led_counter && led_counter < 5) {
                PORTF = gpsbits | 80;
            }
            else {
                led_counter = 0;  
            }
        }

        else {                                                                                     //Medium Voltage Override, flash left to right 2x per sec
            led_counter++ ;
            if ( 0 < led_counter && led_counter < 6 ) {
                PORTF = gpsbits | 160;
            }
            else if (5 < led_counter && led_counter < 11) {
                PORTF = gpsbits | 80;  
            }
            else led_counter = 0;  
         }
       }
       else PORTF = gpsbits | 0;                                                           //Unarmed, turn off all legs

    #endif
    }

    void userhook_50Hz(void)
    {
        // put your 50Hz code here
    }

This reply was deleted.