Hello and welcome to our community! Is this your first visit?
Register
Page 1 of 7 123 ... LastLast
Results 1 to 10 of 64
  1. #1
    Flight Cadet abarton's Avatar
    Join Date
    Oct 2013
    Location
    Atlanta, Georgia, USA
    Posts
    75
    Downloads
    0
    Uploads
    0
    Reputation Points (Add)
    0

    Altitude Hold (again)

    Hody folks! My altitude hold seems to be working very poorly. I know this has been discussed here a few times, especially about a year ago. But enlightenment about how to make it work for me was not forthcoming from older posts. When I was very young, my mother sat me on her knee and said, "Zan, when all else fails, read the source." So I did. And I discovered a few things about atltiude hold that surprised me.

    o At the time altitude hold is activated, if both the barometer and range finder are present, and the current range
    finder reading is "within the range finder range", then altitude hold (AH) will use the range finder (exclusively),
    else it will use the barometer (exclusively).

    o "Range finder range" is determined by the variables maxRangeFinderRange and minRangeFinderRange. They default to 4.5 and 0.
    They're written to EEPROM, but oddly never read from it.

    o The Wiki and the Configurator refer to "the" altitude hold PID. But, that PID applies only when AH is using the barometer.
    When AH is using the range finder, it uses a different PID. This PID is not available through the serial interface and isn't stored
    in EEPROM.

    Sanity check me please. Have I got all that right? :-)

    In my situation, my range finder starts producing essentially random values once I get above about 4.5m.

    So, if I activate AH well below 4.5m, intending to hold based on the range finder, I can tune "the" altitude PID
    all day long and have no effect. If I activate AH well above 4.5m, intending to use the barometer, I really don't know
    which device AH is using because the device choice was based on essentially random values coming from the range finder.

    If I'm not too off-base above, my plan is to forget (disable) the sonar for the moment and see if I can understand why
    barometric hold isn't working well.

    -Alexander
    Last edited by abarton; 01-31-2014 at 02:28 AM.

  2. #2
    Senior Pilot Wandroid's Avatar
    Join Date
    Apr 2012
    Location
    SF Bay Area
    Posts
    104
    Downloads
    0
    Uploads
    0
    Reputation Points (Add)
    0
    Very interesting info, thanks. I'm just trying to get my rangefinder working. I don't
    see any more updates on this, have you confirmed this behavior and can you suggest
    a workaround? ( I suppose I could try changing the code 8) ).

    You mention a separate PID for the rangefinder under 4.5m that doesn't expose the
    gains for tuning. Should I expect these hardwired gains to be reasonably stable
    for say a typical Cyclone config? Just wondering what to expect when I flick the
    switch below 4.5m.

    " If I activate AH well above 4.5m, intending to use the barometer, I really don't know
    which device AH is using because the device choice was based on essentially random values coming from the range finder."

    So you are saying that when both devices are enabled, it will ALWAYS us the rangefinder to determine which sensor
    to use?

  3. #3
    Senior Pilot wooden's Avatar
    Join Date
    Dec 2011
    Location
    Pacific Northwest
    Posts
    4,866
    Blog Entries
    1
    Downloads
    1
    Uploads
    0
    Reputation Points (Add)
    27
    Unless I'm mistaken, the rangefinders output a pretty stable signal when they don't see anything within range and this is well detected by the software. I need to revisit the code handling this but in my experience the transition between the two devices was not poor, barometer-only based altitude hold just doesn't work well.

    I'll try to look at this code tonight, maybe make a block diagram or write up some pseudo-code.
    push the envelope, watch it bend

  4. #4
    Flight Cadet abarton's Avatar
    Join Date
    Oct 2013
    Location
    Atlanta, Georgia, USA
    Posts
    75
    Downloads
    0
    Uploads
    0
    Reputation Points (Add)
    0

    Post

    Wandroid,

    Yes, I'm still working on this. And making progress.

    For the record, I was mistaken about the rangefinder PID. There are indeed two PID instances in memory. But, at boot, the rangefinder's PID constants are copied from the barometer's PID constants, except that P is doubled. So, when you're setting the baro's PID constants, you're also indirectly setting the sonar's.

    My sonar senses well between 0.25m and 4.5m. Much outside that range it produces essentially random values. I'm not sure if that's due to poor mounting or buying the wrong device, or what. Yours may behave completely differently. I'm mostly ignoring my sonar for the moment.

    My workaround is to make AUX1 a three-position switch for altitude hold: off, barometer-based, and sonar-based. At least, this way I know which sensor I'm using.


    Quote Originally Posted by Wandroid View Post
    So you are saying that when both devices are enabled, it will ALWAYS us the rangefinder to determine which sensor
    to use?
    Yes. In processAltitudeHold(), if the sonar claims it can see the ground, then it gets to control the throttle. Else, the barometer is in control. This decision is re-evaluated 50 times per second.

    Code:
    void processAltitudeHold()
    {
      // ****************************** Altitude Adjust *************************
      // Thanks to Honk for his work with altitude hold
      // http://aeroquad.com/showthread.php?792-Problems-with-BMP085-I2C-barometer
      // Thanks to Sherbakov for his work in Z Axis dampening
      // http://aeroquad.com/showthread.php?359-Stable-flight-logic...&p=10325&viewfull=1#post10325
    
      if (altitudeHoldState == ON) {
        int altitudeHoldThrottleCorrection = INVALID_THROTTLE_CORRECTION;
        // computer altitude error!
        #if defined AltitudeHoldRangeFinder
          if (isOnRangerRange(rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX])) {
            if (sonarAltitudeToHoldTarget == INVALID_RANGE) {
              sonarAltitudeToHoldTarget = rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX];
            }
            altitudeHoldThrottleCorrection = updatePID(sonarAltitudeToHoldTarget, rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX], &PID[SONAR_ALTITUDE_HOLD_PID_IDX]);
            altitudeHoldThrottleCorrection = constrain(altitudeHoldThrottleCorrection, minThrottleAdjust, maxThrottleAdjust);
          }
        #endif
        #if defined AltitudeHoldBaro
          if (altitudeHoldThrottleCorrection == INVALID_THROTTLE_CORRECTION) {
            altitudeHoldThrottleCorrection = updatePID(baroAltitudeToHoldTarget, getBaroAltitude(), &PID[BARO_ALTITUDE_HOLD_PID_IDX]);
            altitudeHoldThrottleCorrection = constrain(altitudeHoldThrottleCorrection, minThrottleAdjust, maxThrottleAdjust);
          }
        #endif
        if (altitudeHoldThrottleCorrection == INVALID_THROTTLE_CORRECTION) {
          throttle = receiverCommand[THROTTLE];
          return;
        }

    On a related subject, I don't see how the current implementation of altitude hold could possibly work. (Even without noisy barometers and lying range finders. ) The P term of the altitude PID (either baro or sonar) adjusts throttle based on the difference between current and desired altitude such that the vehicle is always accelerating toward the desired altitude. Vertical speed is greatest just as it crosses through the target altitude -- exactly when we want the speed to be 0. So it overshoots, tries to compensate in the other direction, and winds up in a big lazy oscillation. I tried a big D (like -2400) to try to sap the built-up momentum, and it almost worked, except that the baro noise was hugely amplified and the throttle went crazy.

    What's working well for me currently is a two stage control:

    0) Set baro smooth factor to 0.10 or 0.15.
    1) A speed PID (P=0.5) that sets a target speed based on altitude error. (Thus, move toward desired altitude, reducing speed as we approach.)
    2) A throttle PID (P=50) that adjusts the throttle to match the speed target.
    3) Added a throttleAdjust smoothing factor (= 0.2).

    I need to play with tuning more. Challenges are: the baro is very noisy, and attempts to smooth it by averaging recent values results in a measurement that trails reality by a significant fraction of a second, which invites oscillations.

    -Alexander

    - - - - - - - - - - The following text was automatically merged with this post to prevent doubleposts - - - - - - - - - -

    Wooden,

    I'm using the less expensive rangefinder available on the AQ store. (That must make it the Maxbotix LV-EZ0.) But there's also the more expensive XL-Maxsonar EZ0 which claims "higher resolution, longer range, higher power output and better calibration". Maybe I should got that one? What model sonar are you using that's working well for you?

    -Alexander

  5. #5
    Senior Pilot MikeD's Avatar
    Join Date
    Apr 2011
    Location
    Waterlooville, Hampshire, UK
    Posts
    1,024
    Downloads
    4
    Uploads
    0
    Reputation Points (Add)
    6
    The spec for the LV-EZ0 (http://www.maxbotix.com/documents/MB1000_Datasheet.pdf) states that it should provide range readings between 6 and 254 inches but can detect objects at zero inches.
    It also states that "Range data can be acquired once every 49mS." ie 20Hz
    It doesn't say what you get if you read quicker than that - do you get a repeat of the last reading or something else!

  6. #6
    Senior Pilot wooden's Avatar
    Join Date
    Dec 2011
    Location
    Pacific Northwest
    Posts
    4,866
    Blog Entries
    1
    Downloads
    1
    Uploads
    0
    Reputation Points (Add)
    27
    I have both models but have not used the XL yet. LV worked just fine as far as I can remember.

    And as far as I can tell, when sonar is in range, it uses both baro and sonar. There is no "if else," it's all "if". If sonar is in range, it applies it before applying baro adjustments, otherwise it's just baro:
    Code:
    if (altitudeHoldState == ON) {    int altitudeHoldThrottleCorrection = INVALID_THROTTLE_CORRECTION;
        // computer altitude error!
        if (isOnRangerRange(rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX])) {
            if (sonarAltitudeToHoldTarget == INVALID_RANGE) {
                sonarAltitudeToHoldTarget = rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX];
            }
            altitudeHoldThrottleCorrection = updatePID(sonarAltitudeToHoldTarget, rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX], &PID[SONAR_ALTITUDE_HOLD_PID_IDX]);
            altitudeHoldThrottleCorrection = constrain(altitudeHoldThrottleCorrection, minThrottleAdjust, maxThrottleAdjust);
        }
        if (altitudeHoldThrottleCorrection == INVALID_THROTTLE_CORRECTION) {
            altitudeHoldThrottleCorrection = updatePID(baroAltitudeToHoldTarget, getBaroAltitude(), &PID[BARO_ALTITUDE_HOLD_PID_IDX]);
            altitudeHoldThrottleCorrection = constrain(altitudeHoldThrottleCorrection, minThrottleAdjust, maxThrottleAdjust);
        }
        if (altitudeHoldThrottleCorrection == INVALID_THROTTLE_CORRECTION) {
            throttle = receiverCommand[THROTTLE];
            return;
        }
    }
    push the envelope, watch it bend

  7. #7
    Flight Cadet abarton's Avatar
    Join Date
    Oct 2013
    Location
    Atlanta, Georgia, USA
    Posts
    75
    Downloads
    0
    Uploads
    0
    Reputation Points (Add)
    0
    Of the three inner if-blocks, the second sets the throttle correction based on the barometer. But only if the first block hasn't already set the throttle correction, based on the sonar, to something other than the sentinel value INVALID_THROTTLE_CORRECTION.

    -Alexander

  8. #8
    Senior Pilot wooden's Avatar
    Join Date
    Dec 2011
    Location
    Pacific Northwest
    Posts
    4,866
    Blog Entries
    1
    Downloads
    1
    Uploads
    0
    Reputation Points (Add)
    27
    Derp.

    I think that's kind of a poor implementation in the first place. We should be using the sonar and baro data in conjunction to figure out an altitude and apply corrections based on a "more trusted" altitude. Sonar should be used to "validate" the baro readings, in essence.

    But really, the baro and sonar should be used to validate accelerometer-based altitude estimates.
    push the envelope, watch it bend

  9. #9
    AeroQuad Flight Control Expert
    Join Date
    May 2010
    Location
    Albuquerque, NM
    Posts
    1,410
    Downloads
    1
    Uploads
    0
    Reputation Points (Add)
    5
    Actually, the best way to do this is as follows:

    1)Collect about 30 seconds of baro based altitude while the system is initializing. Average this a call it altitude0. Everywhere baro altitude is used, subtract this from the sensor reading. All altitudes are now referenced from the takeoff position. A negative altitude means you have descended below the takeoff point. No problem with this.

    2)I don't recall the exact numbers, but say the ultrasonic transducer is valid from 0 to 40 feet. I'm going to exclusively use ultrasonic transducer (AGL) altitude below 20 feet. From 20 to 30 feet I'll start blending in baro, above 30 I'm exclusively on baro.

    3)Constantly monitor the AGL altitude and it's validity. When it becomes valid, start a reverse blend to transition back to AGL altitude. This way it does not matter if the landing point is different from the takeoff point.

    There's a little more to this, but I've used this scheme before on big birds with radar altimeters and air data computers. Just need to simplify things a little for this application.

    BLueSKy RC Mini Quad, Turnigy 1811-2000 motors, Turnigy 6 amp ESCs with BLHeli 10.2, 2s, Naze32, Spektrum Satellite, FF32lite software
    Fortis Airframes Titan TriCopter, BP2212-13 motors, HobbyWing 18 amp ESCs with BLHeli 10,2, 3s, Naze32Pro, Spektrum Satellite, FF32mini software
    AeroQuad Typhoon, BP2212-13 motors, HobbyWing 18 amp ESCs with BLHeli 10,2, 3s, AQ32, Dual Spektrum Satellites, 2.4 GHz xBee telemetry, OpenLog FDR, FF32 software
    DJI550 Hex, BP2217-9 motors, HobbyWing 30 amp ESCs with BLHeli 10.2, 3s, AQ32, Dual Spektrum Satellites, OpenLog FDR, FF32 software

  10. #10
    New Recruit
    Join Date
    Dec 2013
    Posts
    17
    Downloads
    0
    Uploads
    0
    Reputation Points (Add)
    0
    hey guys additional question on the altitude hold...
    I have bought Maxsonar sensor EZ4 using I2C communication, as it's different from the aeroquad one which used output analog signal, I have written a code to read the distance.

    Code:
    #ifndef _AEROQUAD_MAX_SONAR_RANGE_FINDER_EZ4_H_#define _AEROQUAD_MAX_SONAR_RANGE_FINDER_EZ4_H_
    
    
    #if defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(BOARD_aeroquad32)
    
    
    #include "RangeFinder.h"
    
    
    #define SPIKE_FILTER_MARGIN 300 // mm ; changes bigger than this need two samples to take effect
    
    
    #define RANGER_COUNT ((sizeof(rangeFinders) / sizeof(struct rangeFinder)))
    
    
    //The Arduino Wire library uses the 7-bit version of the address, so the code example uses 0x70 instead of the 8‑bit 0xE0
    #define SensorAddress byte(0x70)
    //The Sensor ranging command has a value of 0x51
    #define RangeCommand byte(0x51)
    //These are the two commands that need to be sent in sequence to change the sensor address
    
    
    //FIR Filter Variable
    float first_previous = 0;
    float second_lastRange = 0;
    
    
    // last reading used for 'spike' filter
    short lastRange;
    
    
    void takeRangeReading();
    word requestRange();
    float thirdorder_fir (float , float , float );
    
    
    void inititalizeRangeFinders() {
    	takeRangeReading();
    }
    
    
    void updateRangeFinders() {
    
    
      float range = (float) (requestRange()	*	(10)	);
    
    
      range = thirdorder_fir(range, lastRange, second_lastRange);
      rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX] = (float)range / 1000.0;
      
      // Following will accept the sample if it's either withing "spike margin" of last raw reading or previous accepted reading
      // otherwise it's ignored as noise  
      if ((abs(range - lastRange) < SPIKE_FILTER_MARGIN)) {
        rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX] = (float)range / 1000.0;
      }
      second_lastRange = lastRange;
      lastRange = range;  
        
      takeRangeReading();
    }
    
    
    //Commands the sensor to take a range reading
    void takeRangeReading(){
           Wire.beginTransmission(SensorAddress);             //Start addressing 
           Wire.write(RangeCommand);                             //send range command 
           Wire.endTransmission();                                  //Stop and do something else now
    }    
    
    
    //Returns the last range that the sensor determined in its last ranging cycle in centimeters. Returns 0 if there is no communication. 
    word requestRange(){ 
        Wire.requestFrom(SensorAddress, byte(2));
                if(Wire.available() >= 2){                            //Sensor responded with the two bytes 
               byte HighByte = Wire.read();                        //Read the high byte back 
               byte LowByte = Wire.read();                        //Read the low byte back 
               word range = word(HighByte, LowByte);         //Make a 16-bit word out of the two bytes for the range 
               return range; 
            }
            else { 
            return word(0);                                        //Else nothing was received, return 0 
        }
    }
    
    
    float thirdorder_fir (float current_value, float first_previous, float second_previous) {
    	float first_coefficient = 0.6;
    	float second_coefficient = 0.3;
    	float third_coefficient = 0.1;
    	
    	return (first_coefficient*current_value + second_coefficient*first_previous + third_coefficient*second_previous);
    }
    #endif 
    #endif
    The code successfully read the distance value ( the "range" and "rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX" are correct], but somehow I cannot do the altitude hold with the quad / _ \
    Can anyone point out my mistakes >.<?


 
Page 1 of 7 123 ... LastLast

Similar Threads

  1. Altitude Hold
    By RogerTil in forum General Discussion
    Replies: 3
    Last Post: 04-11-2013, 01:08 AM
  2. Altitude Hold
    By jysnchl in forum AeroQuad Configurator
    Replies: 17
    Last Post: 06-08-2012, 11:09 PM
  3. Altitude Hold problem in 3.0.1
    By xxxsandman in forum AeroQuad Flight Software
    Replies: 12
    Last Post: 04-18-2012, 02:41 AM
  4. Altitude Hold
    By Hex in forum AeroQuad Flight Software
    Replies: 2
    Last Post: 09-24-2011, 03:36 PM

Bookmarks

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •