Airplane mode RTH

rbirdie001
Posts: 178
Joined: Fri Apr 01, 2011 10:32 pm
Location: Czech Republic, Prague

Re: Airplane mode RTH

Post by rbirdie001 »

Hi Patrik and Andrej,
testing results are really interesting but for my thinking about it I still miss some anwers about the expected behaviour of RTH algoritm.
Most info I'm getting from here:
Stage 1
Start to navigate.
CLIMBTHROTTLE & GPS_MAXCLIMB untill plane reached GPS_ALT_HOLD

Stage 2
Navigate to home
CRUICETHROTTLE, Maintain GPS_ALT_HOLD .

Stage 3
CRUICETHROTTLE, Maintain GPS_ALT_HOLD and Cirkulate around homepos until
RTH id disabled or FAILSAFE_OFF_DELAY

But this description is maybe already obsolete and behaviour of my plane doesn't much correspond to it.
Patrik, can you please once more revise the algoritm and describe as much in details as possible what should be happening in each stage in last version?
Namely what is "setpoint" for the height in each stage and what should be happenning when the actual height is lower and what if higher than setpoint.
As I tested formely here in this video: http://www.youtube.com/watch?v=Ux266uz0WQ0, the GPS heigh isn't very reliable but for our purpose it should be sufficient.
I have really strong feeling, that this pitching down effect is caused by "request" to lower altitude (probably to zero) but I think that after reaching home there should be two options:
1) Cicle around home and maintainn safe height (e.g. 50m) until the maximum time is reached (old version does it?). This time can be quite long (e.g. 10 min) to give you time do something (e.g. change battery in TX), after that time to do the same as 2) will do immediately
2) After reaching home disarm (new version does it?) and in circles slowly descend to landing.
Patrik, when you have time, please look at questions in my last posts. I can't read answers in the code but it would help me underststand it and maybe to have more usefull ideas... :?
@ Andrej: Your video is interesting, I have feeling that there can be the same pitching up behaviour as I have, but unfortunately I'm little confused what mode you have activated at what time. :?: Could you choose one or two typical flights and put simple subtitles like " RTH activation" or "Passthru and manual control".
This would help a lot to understand what's happening.
Thanks!
Further testing have to wait again - to Czech came another return of winter - new snow, strong wind and cold. But spring is scheduled in 3 days! :D
See you!
Roman
P.S. Sorry for too many questions, all my life I have problems that I'm asking too much... ;)

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

@rbirdie001:
Since my video includes OSD (MinimOSD from viewtopic.php?f=8&t=2918) I'll only explain what you can read from it!
All the informations you are asking for are in top-left corner of screen and since I'm only flying in 3 modes it is even easier
- first mode used for hand launch is acro (gyro only mode) in which you can see text "Acro mode" (but it is not used in this video)
- second mode (used during most of video) is "Horizon Mode" for which you can se arrow sign under "ACC" and text "Stable Mode"
- third mode is RTH for which you can see arrow under "GPS" and small House like icon.

In the middle-left part of the screen you can see GPS relative altitude (alt. above start point).
In the top-right corner you can observe three numbers that mean (from top to down) distance to home, GPS (ground) speed, absolute alt. (above sea level).

Hope that explains all!

Best Regards
Andrej

rbirdie001
Posts: 178
Joined: Fri Apr 01, 2011 10:32 pm
Location: Czech Republic, Prague

Re: Airplane mode RTH

Post by rbirdie001 »

Thanks Andrej,
sorry, I don't know layout of minim OSD, I use Remzibi with my own simplified layout (see my video in previous post).
I'll watch again your video and see if it will now explain something to me...
Roman

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

I would say that in my case GPS ALT accuracy is much better and faster than yours (you are flying near buildings) but otherwise I agree that GPS has some lag.
I do not know how much ALT. accuracy brings Ublox binary protocol parsing?!

Regards Andrej

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

@PatrikE
could it be possible that this line (around line 1349 in MultiWii):

Code: Select all

if (GPS_distanceToHome >100){ GPS_angle[PITCH]=0 ;}       // RTH Nav Stage No elevator input to correct altitude

creates vertical turn when wing/plane is further than 100m from home?

If the line above (line 1331) calculates proper turning ELEVATORCOMPENSATION

Code: Select all

GPS_angle[PITCH]+=(angle[ROLL]/100 * ELEVATORCOMPENSATION); // Compensate elevator compared with rollAngle


that works during normal turns than first mentioned line removes that correction altogether! Right!?

Regards
Andrej

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

Today's test went perfectly, but due to strong winds and limited area I have not tested before-mentioned correction (commented out) code path since I flew less than 100m away or less than 50m high.
So inside 100m radius it works perfectly (apart from not really explainable TH bursts) even in stronger winds.

Regards
Andrej

rbirdie001
Posts: 178
Joined: Fri Apr 01, 2011 10:32 pm
Location: Czech Republic, Prague

Re: Airplane mode RTH

Post by rbirdie001 »

Hi Andrej,
I again watched your videos, now I understand better the flight situations.
I see generally that once the plane gets to fast descending, it usually doesn't want to recover from it.
I think that this is not due to GPS lag because numbers shown on the OSD are coming also from the GPS (probably the same one) and are clearly showing fast descending, but MWC doesn't react and pull the elevator...
By side of problem you described I can see something similar to my problem:
At your video the plane flies with RTH active at stable level about 50m between youtube time 0:52-1:06 and then at once at 1:06 starts fast descending. It's descending from 50 meters at 1:06 until 12meters at time 1:20 and then is a video cut. What was then? I guess you had to disable RTH and recover plane manually.
I think that lowering maximal angle for descending could help, but there is probably something wrong with the GPS_angle[PITCH] at distances from home under 100m.
I tried to look into code below alone and have following idea:

Code: Select all

if (f.GPS_HOME_MODE && !f.CLIMBOUT_FW && GPS_AltCorr <=0 ){ //If we are higher than RTH alt.
          if (GPS_distanceToHome >100){ GPS_angle[PITCH]=0 ;}       // RTH Nav Stage No elevator input to correct altitude
          if (GPS_distanceToHome <100){ rcData[THROTTLE]= 1200;}    // RTH Decend only within 100 meters

When the plane is under 100m from home, the second line is not executed anymore but third line control only throttle so GPS_angle[PITCH] is not refreshing and there remains some old value which keeps elevator down and plane is falling...
What do you think?
Roman

Mis
Posts: 203
Joined: Fri Apr 01, 2011 12:23 am

Re: Airplane mode RTH

Post by Mis »

PartickE:
Look at my version of airplane RTH. This control motor for get predefined speed (from GPS).

Code: Select all

   // Navigation with Planes   
   /************************************************************/ 
      static int16_t Auto_Thro = 1300;
      uint32_t milis = millis();
      if(milis > thr_time) {
        thr_time = milis + 10;              // 10ms refresh for throttle regulation
        if(nav[2] > 50)  Auto_Thro--;
        if(nav[2] < -50) Auto_Thro++;
        Auto_Thro = constrain(Auto_Thro , MINTHROTTLE+1, MAXTHROTTLE-50);
        if(GPS_distanceToHome < 10          // home reached (10 meters)
        #if defined(VBAT)
          || (vbat < batAlarm && vbat > 16) // if battery is Critical
        #endif
        ) {
          Auto_Thro = MINTHROTTLE;          // turn off engine and elevator for safety stabilisation
          thr_time = 0xFFFFFFFF;            // turn off throttle regulation
        }
      }
      if(Auto_Thro == MINTHROTTLE) nav[1] = 0;  // Do not correct pith during glide

      // Mix & limit outputs
      int16_t dirlimit = (int16_t)conf.D8[PIDNAVR] * 10;
      GPS_angle[ROLL]  = (nav[0]*conf.P8[PIDNAVR])/10;
      rcCommand[YAW]  += constrain(GPS_angle[ROLL]/3 , -dirlimit, dirlimit );  // rudder
      GPS_angle[ROLL]  = constrain(GPS_angle[ROLL]   , -dirlimit, dirlimit );  // ailerons

      uint16_t bankKomp = abs(GPS_angle[ROLL]) * ELEVATORCOMPENSATION / 100;
      dirlimit = (int16_t)conf.D8[PIDALT] * 10;
      GPS_angle[PITCH] = ((nav[1] * conf.P8[PIDALT])/10) - bankKomp;
      GPS_angle[PITCH] = constrain(GPS_angle[PITCH], -dirlimit, dirlimit);

      if((rcData[THROTTLE]>1800 && GPS_speed>MIN_SPEED*27) || f.FAILSAFE_ON)  // if THROTTLE stick is at MAX and speed > critical, or failsafe is ON, use auto THROTTLE
        rcCommand[THROTTLE] = Auto_Thro; 


and in GPS.ino:

Code: Select all

static void GPS_calc_nav_FW() {
  int16_t GPS_FwError, rth_speed;

  rth_speed = min(CRUICE_SPEED*28, wp_distance / 5);
  rth_speed = max(rth_speed, MIN_SPEED*28);
  nav[2] = GPS_speed - rth_speed;                // speed error

  if(f.MAG_MODE && !f.SMALL_ANGLES_25)
    GPS_FwError = target_bearing/100 - heading;    // Dir error in deegres from MAG
  else
    GPS_FwError = target_bearing/100 - GPS_ground_course/10;    // Dir error in deegres from GPS

  // wrap headingerror
  if (GPS_FwError <= - 180) GPS_FwError += 360;
  if (GPS_FwError >= + 180) GPS_FwError -= 360;
  nav[0] = GPS_FwError;
  nav[1] = (GPS_altitude*10) - (AltHold/10);      // nav[1] in 0.1m
}


nav[0] is heading error
nav[1] is altitude error
nav[2] is speed error

AltHold is return altitude in cm (memorized at RTH seith on, or predefined in failsafe mode).

CRUICE_SPEED and MIN_SPEED are defined in config.h and have units in km/h
P of ALT PID is the P for height control. D is pitch angle limit.
P of NavR PID is the P for heading control. D is roll angle limit.

During normal RTH, if you set THROTTLE at maximum (>1800) you get automatic throttle regulation for RTH speed = CRUICE_SPEED, if not, you can control throttle manually. During failsafe the auto method is used.

PS. This code is from my little old, custom software based on MWC 2.1 with some ideas based on Your older code, but can be easy adapted to newer versions.

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

@rbirdie001
Watching my video again with your observations in mind...
- it looks like your observations are right, but since I'm using DISARM when at home and glide I can say that I newer felt uncomfortable with flying near and my wing would probably always (emergency) land safely without my intervention.
At 1:20 I disabled RTH (rearm) to continue flying and not to save my wing.

Even when the wing goes into vertical dive turn (more than 50m high and 100m out) it eventually starts to level and slowing down (probably one of magic number crossed), but the speed that gains in the mean time and altitude it looses brings it out of my comfort zone!

@Mis
I'm not an expert on that field but IMO controlling TH with ground speed (from GPS) can be very problematic since when flying in stronger winds (that gets higher with increased alt.) you can get into overspeed situation against wind (if the drive is able to do that) or stall condition with wind direction. In addition when descending or climbing with higher angles the ground speed is only part of true air speed and since vertical speed estimation with GPS is pore you can easy get into overspeed situation.

Best Regards
Andrej

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

rbirdie001 wrote:Hi Andrej,
When the plane is under 100m from home, the second line is not executed anymore but third line control only throttle so GPS_angle[PITCH] is not refreshing and there remains some old value which keeps elevator down and plane is falling...
What do you think?
Roman


In MWII way of programming (and probably elsewhere) you must look wider scope of code and observe all lines that possibly change your observed value:
- and thus in line 1330 and 1331 the code calculates needed elevator correction from roll input and predefined constant from config.h
- and than if alt. must be changed it recalculates/changes it in line 1340 from PID'ed value for correction and maximum allowed correction from config.h
- and than if not (l. 1345 ) and alt > 50m (l 1348) and if GPS_distanceToHome >100 (l. 1349) it discharge all above calculations (otherwise valid number is one from the above).

So if I'm not missing something below those lines that last line (l. 1349) creates turn without elevator correction (if plane is more than 50m high and more than 100m out).

Best Regards
Andrej

rbirdie001
Posts: 178
Joined: Fri Apr 01, 2011 10:32 pm
Location: Czech Republic, Prague

Re: Airplane mode RTH

Post by rbirdie001 »

Hi!
First I'd like to correct myself: my reading of Patrik's altitude control code was probably wrong (I've just started learning C language) so I missed that lines I pointed out were just "correcting" lines and main calculation for elevator should be executed always. I anyway still think that there is something wrong in alt control, but not that simple... :(
@Andrej - thanks for explanation about your video. I can tell you that my plane was falling down "very uncomfortably" for me.. :shock:
@Mis -Andrej is right that GPS data are very tricky (speed because of wind and height always) so my recommendation would be let plane "fly alone" as much as possible, because eg. my Easystar with GYRO and ACC flies without any control perfectly and only watch for maximals. I'm very interested in testing your code but I don't dare merging it myself - there is big risk of making mistake.
I'll wait when someone with enough knowledge will do it.
@Patrik- Our "guru" we are waiting for your return - where are you? :D :D :D
Regards
Roman

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

@rbirdie001
In the mean time I edited my replay (couple of times) so if you skim thru it and maybe get some new ideas.

Regards Andrej

BTW: I'm also far from fluent in C/C++ programing but I'm doing my best :)

Mis
Posts: 203
Joined: Fri Apr 01, 2011 12:23 am

Re: Airplane mode RTH

Post by Mis »

I agree that the use of GPS as a source of speed is not ideal. But without Pitot tube there is no better option. GPS speed is better than the throttle set to a constant value, because if you set throttle too low the wind will fly back, and no chance of any control.

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

Hi Guys.

Sorry for the absens for a coupple of days..
Other thing need attention to;)

I see your'e been busy with testing.

Iw'e cuickly read thru the posts.

Some answer first.
The Nav codeBlock starts with calculating NavErrors.
Direction & Altitude.
AltError is relative to Target. ( Positive => Higher)

The throttle Controll contains some errors to correct.
But the ide is to compensate throttle with (AltError * scalefator) ex. -5m * 10 will add 50µs to throttle
To get suitable compensating depending on Alterror.
If there's no Alterror throttle will be in Cruice.

The if's i added at last update only overwrtes the values only if the case is true.
Otherwise the values is left as calculated originally.
The current If's
1, If dist is more than 100m Navigate home Use ACC only for AltitudeCorrection.
2, Within 100m Descend to RthAlt and Repetedly nav home at RthAlt
3,After Hitting Home Pos Lat,Lon & Alt Disarm and glide.



@MIS
Yeah i recognice some parts of the code. ;)
As pointed earlier Groundspeed and airspeed can diff a lot.
In this ver i wanted a stripped to the bone code to verify things.

I might be wrong about this But...
nav[1] = (GPS_altitude*10) - (AltHold/10); // nav[1] in 0.1m

GPS_altitude is a integer an if you multiply it by 10 it will only make it 10 times bigger!
The decimals is lost Way back in the GPS code.
You need to make some changes in gps.ino to get Alt in dm.
{GPS_altitude = grab_fields(string,0);} // altitude in meters added by Mis
Change to fields(string,1) to get dm.

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

Mis wrote:I agree that the use of GPS as a source of speed is not ideal. But without Pitot tube there is no better option. GPS speed is better than the throttle set to a constant value, because if you set throttle too low the wind will fly back, and no chance of any control.

So ideali I controller (from p i d) :)
Normally set fixed TH and if there is no ground movement (or into wrong direction) slowly increase up to set max and hope for the best!

Best Regards
Andrej

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

I have some corrections.
Replace th line adding ELEVATORCOMPENSATION in Mwii.ino
line 1358 in my code.

Code: Select all

 if(GPS_AltErr >=0){ GPS_angle[PITCH] = GPS_angle[PITCH]/2; }// Half throw When descending.
     GPS_angle[PITCH]+=(abs(angle[ROLL]) * ELEVATORCOMPENSATION /100); // Add rollAngle to elevator.


Replace the // Throttle controll part with this.

Code: Select all

     // Throttle controll
    #define IDLE_THROTTLE 1200
     if (GPS_AltErr > 2 || GPS_AltErr < 2) {// Deadspan for throttle at desired alt.
        rcData[THROTTLE]=constrain(CRUICETHROTTLE - (GPS_AltErr*10) , IDLE_THROTTLE ,CLIMBTHROTTLE);
     }else{ rcData[THROTTLE]= CRUICETHROTTLE; }


There's a lovely snowstorm outside and the code is only benchTested.

/Patrik

Mis
Posts: 203
Joined: Fri Apr 01, 2011 12:23 am

Re: Airplane mode RTH

Post by Mis »

PatrikE wrote:I might be wrong about this But...
nav[1] = (GPS_altitude*10) - (AltHold/10); // nav[1] in 0.1m

GPS_altitude is a integer an if you multiply it by 10 it will only make it 10 times bigger!
The decimals is lost Way back in the GPS code.
You need to make some changes in gps.ino to get Alt in dm.
{GPS_altitude = grab_fields(string,0);} // altitude in meters added by Mis
Change to fields(string,1) to get dm.

I know, but the GPS_altitude can be getted from ublox, i2c_gps, and always the unit is meters. This is sended to OSD via serial MSP. In this case i can't change the units.
For altitude control I use 0.1m unit, because the altitude can be controlled by baro, not only by GPS.
If baro is on, the "nav[1] = (GPS_altitude*10) - (AltHold/10)" is replaced by "nav[1] = (EstAlt - AltHold)/10L" and the altitude control can be more accurate.

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

Sounds good Mis
Perhaps Baro can be used in planes to.
If it doesn't get affected by airspeed.
That would be great.

dr.tom
Posts: 141
Joined: Fri Mar 30, 2012 4:46 pm
Location: Croatia
Contact:

Re: Airplane mode RTH

Post by dr.tom »

That is much better way than with GPS alt.
gps is good for horizontal movement and speed readings, but in height, it has large oscillations,
while even cheap baro BMP085 can hold plane as accurate as 1m. Tested many times....
I can only imagine how new MS5611 performs :)

baro is especially good for FPV landing, no false distance from ground displayed(home- takeoff location, not some other ground ofcourse :) )...

this osd uses it, BMP085
http://www.foxtechfpv.com/cyclops-storm ... p-688.html

before they used gps, but wasn't so good
http://www.foxtechfpv.com/cyclops-nova- ... p-402.html


I hope to test your code in some of my planes.
it brings a really nice option for a small amount of money(328p 2x1cm board for 9$, some cheap IMU from ebay 10-30$, and a 20-ish GPS from rctimer,
all under 50$ and very small dimensions, can fit into small planes too)
Keep up the good work PatrikE, 8-)
you keep surprising us with your findings in more areas (pwm timings, gimbal, servos, rth....)


Cheers :)
Dr.Tom
http://tinyurl.com/DrTomYoutube

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

Have anyone tested PosHold?
Just to see how good/bad it can hold same altitude.

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

@PatrikE
I can understand your mods.... but still if you first calculate neccesary EL compensation for turning like in

Code: Select all

 GPS_angle[PITCH]+=(abs(angle[ROLL]) * ELEVATORCOMPENSATION /100);

and that will work nicely if the plane is close to home and whatever hight
then something is wrong when the plane is far from home and this line comes in:

Code: Select all

if (GPS_distanceToHome >100){ GPS_angle[PITCH]=0 ;}

because at least FW will not turn nicely when there is no elevator compensation added in banked turns.

Regards
Andrej

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

Good catch Andej,

BankKompensation should be made after the If's.

Code: Select all

 GPS_angle[PITCH]+=(abs(angle[ROLL]) * ELEVATORCOMPENSATION) /100;

// Limit outputs

Place it just before
// Limit outputs

Actually this maby should be added in Acc-Modes even without GPS..!?...

And please add this to disable Autothrottle.

Code: Select all

// Throttle in Passthru without ACC and GPS enabled
    if( !f.ANGLE_MODE && !f.HORIZON_MODE) rcData[THROTTLE]= TX_Thro;
/***************** DEBUG *****************/

Before the first commented ** DEBUG ** last in FW_NAV code.

Many changes..
Maby time for a corrected upload..

dr.tom
Posts: 141
Joined: Fri Mar 30, 2012 4:46 pm
Location: Croatia
Contact:

Re: Airplane mode RTH

Post by dr.tom »

is it possible to take speed into account?

for example if model is going very fast and you turn on RTH, it should'nt make sharp turn or recover from dive to quickly,
it might break it's wings.

rbirdie001
Posts: 178
Joined: Fri Apr 01, 2011 10:32 pm
Location: Czech Republic, Prague

Re: Airplane mode RTH

Post by rbirdie001 »

Hi all!
@dr.tom,
I had also the idea that check speed at least for maximum allowed would be fine, but it's true that speed reading is really so much dependent on the wind, that it's not safe enough. I think that for protecting plane in cases that RTH is activated at high speed could help simple "slow lead-in" for the navigation corrections, i.e. servos will transit from old values to new values during some longer time e.g. 5 sec. This should be sufficient.

Generaly I think that there should be some filtering for navigation control because if you watch my flight record here: viewtopic.php?f=7&t=2456&start=90#p33188 and turn on the sound, you can hear shortly after RTH activation (at about 0:49) servos running fast from one side to the other and back and see plane jerking. Also this could improve (or even more spoil :( ) "snake zigcag" path to home (although for me this isn't a problem ). PID would help here for sure.
Just for complete info - my plane uses both ailerons and rudder and I think that fast moving is especially rudder.

@ Patrik: Wellcome back! :D
Yes, I'd also prefer uploading of a new "version" with all the corrections - I'm afraid that I could patch something wrong. Unfortunately this weekend it's not very likely that I could test new version because it should be again quite cold and I have some family obligations...
regards
Roman

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

PatrikE wrote:Actually this maby should be added in Acc-Modes even without GPS..!?...


That is probably not good idea since it will change the way that users are perceiving planes (differently than copters).
For all normal plane/wing it is normal that you apply elevator in banked turn and omit it when doing roll's or you want to simply loose height in relaxed turn. With all copters (CP or multi) you add throttle/collective when you want to do bank turn and there is also no code to "help" you with that in any mode.

Regards Andrej
Last edited by crashlander on Thu Mar 21, 2013 8:46 pm, edited 1 time in total.

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

Your'e right Andrej.

Better to keep it in the auto modes.

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

DELETED....

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

Updated code with the latest changes.
FW_Nav_130322.rar

Some new setting added.

Code: Select all

// ....!!! New Defs !!!....
#define IDLE_THROTTLE 1200  // Lowest throttleValue during Descend
#define SCALER_THROTTLE 8   // Adjust to Match Power/Weiget ratio of your model
#define RTH_BAILOUT  false  // Forced RTH Climbout with Level Wings
#define FAILSAFE_RTH false  // Enable RTH for failsafe incl Auto DisARM at home

I have Re aranged the code and moved the FW Nav code to a separate file temporarily.
It's easier to debug if it's isolated.
More comments is also added in the code.

RTH alt is set from Gui. (ALT D term)
Default 50m.

I have only BenchTested it yet.

crashlander wrote:DELETED....
Ooops

edit.
Added link

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

@PatrikE
What do you think about idea to maintain ALT if is higher than predefined since if I fly much higher there must be a reason for that eg.

Code: Select all

uint8_t set;    
     // Altitude controll
     int currAlt = GPS_home[ALT] - GPS_altitude;  // Altitude over home
     
   if(!f.GPS_HOME_MODE){set=0;} 
   if(currAlt > GPS_hold[ALT] && set==0 && f.GPS_HOME_MODE){
        GPS_hold[ALT]=GPS_altitude;;   //if currently higher than predefined target take that for new target
        set=1;
   }

Also not tested....
Regards
Andrej

EDIT: I guess it will not work that way since the code above will over write it... ...but that is the basic idea.

BTW: Maybe that is already in your new code but have not checked yet!
Last edited by crashlander on Fri Mar 22, 2013 9:14 pm, edited 3 times in total.

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

PatrikE wrote:
crashlander wrote:DELETED....
Ooops

It was stupid question.... :) before I read your code.

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

Not a bad idé I'll take a look at it.

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

Small Bug fixed For FlyingWing.
File updated.
FW_Nav_130322.rar

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

Today air-tested (2 packs, between 20 and 30min of flying and returning home) new RTH code and it works without issues.
Very good job Patrik!

Regards Andrej

msev
Posts: 186
Joined: Thu Apr 14, 2011 11:49 am

Re: Airplane mode RTH

Post by msev »

Nice work guys!

This sensor could be of use on a plane also: http://www.rctimer.com/index.php?gOo=go ... oductname=
(Rctimer Airspeed Sensor MPXV7002DP )

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

msev wrote:Nice work guys!

This sensor could be of use on a plane also: http://www.rctimer.com/index.php?gOo=go ... oductname=
(Rctimer Airspeed Sensor MPXV7002DP )


Already done by @KaiK.
viewtopic.php?f=8&t=2450
You just need to merge the cahangas he made.
And add True Cruise conroll..

Peter
Posts: 82
Joined: Mon Jun 11, 2012 2:09 pm

Re: Airplane mode RTH

Post by Peter »

That is great news! They don't sell a pitot tube, can they be used without?

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

You need a tube to reach outside the plane...
http://store.diydrones.com/Kit_MPXV7002DP_p/kt-mpxv7002dp-01.htm


Edit Corrected link.
Last edited by PatrikE on Mon Mar 25, 2013 12:37 pm, edited 1 time in total.

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

Saturday's tests of RTH in video.
RTH Altitude was set around 24m (my mistake of not reading the code properly and leaving D term of ALT Pid unmodified ) and

Code: Select all

#define FAILSAFE_RTH true


Possible improvement for faster planes/wings would be to use higher devisor in

Code: Select all

if(GPS_AltErr >=0) GPS_angle[PITCH] = GPS_angle[PITCH]/3;

or maybe to make it as define!?


http://www.youtube.com/watch?v=KeBt8phV ... BC&index=6

rbirdie001
Posts: 178
Joined: Fri Apr 01, 2011 10:32 pm
Location: Czech Republic, Prague

Re: Airplane mode RTH

Post by rbirdie001 »

Hi guys,
loaded new 130322 version and waiting for better weather (here still snowing, windy and freezy :( hopefully at Eastern weather may improve...).
I don't understand very well what that option should do:

Code: Select all

#define RTH_BAILOUT  true  // Forced RTH Climbout with Level Wings

Can someone explain in other words?
One more ask:
As recommended I left auto disarm when home reached @failsafe active but I don't understand how to properly select conditions for ARM. I can't use arm via sticks in the air and when I select e.g. AUX1 LOW box for arm, it always disarms when I put throttle down while AUX1 isn't low. This I see as dangerous because I can get confused and crash the plane because of disarmed motor.
I have only AUX1 (0=passthru and 1=STABILIZATION) and AUX2 (0=GYRO 1/2=ACC 1=RTH). What is recommended way of arming in that config?
Thanks!
Roman

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

rbirdie001 wrote:I have only AUX1 (0=passthru and 1=STABILIZATION) and AUX2 (0=GYRO 1/2=ACC 1=RTH). What is recommended way of arming in that config?

Maybe you can mix two two-position switches so that one moves from max to mid and the other moves to min!?
On Spektrum DX7 it goes that way. And than you can have:
3 state switch as 0 - passthu, 1 - gyro(acro), 2 - level/horizon
1st 2 state switch ARM
2nd 2 state switch ARM+RTH
With that approach I only disable RTH routine and plane is armed again!

Have not tested it but I believe that

Code: Select all

#define RTH_BAILOUT  true
means that until plane reaches set height it will not turn (navigate) but climb in straight line.

Regards Andrej

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

You can also use RTH_BAILOUT as a autoLaunch mode.
Just enable RTH put the radio on the ground and launch the plane
It will climb in stright line until RTH altitude is reached and return home after that.

rbirdie001
Posts: 178
Joined: Fri Apr 01, 2011 10:32 pm
Location: Czech Republic, Prague

Re: Airplane mode RTH

Post by rbirdie001 »

Hi guys,
thanks both for great ideas!
@Andrej:
3 state switch as 0 - passthu, 1 - gyro(acro), 2 - level/horizon
1st 2 state switch ARM
2nd 2 state switch ARM+RTH
With that approach I only disable RTH routine and plane is armed again!
Yes, that configuration is exactly what I needed, my mind was just "locked" in the idea, that AUX1 must be PASSTHRU so this solution didn't come to my mind... ;)
(I set the 2 state switch to 0=ARM and 1=RTH only, I guess that this you ment and you just made mistake describing it...)

@Patrik:
Using bailout as a auto launch is also grat idea! I can imagine how "old school modellers" will be surprised seeing that :D .
I'm really curious testing that all, but beside that I'm currently ill, outside is snowing and next days shouldn't be much better :(
Repeating the same again, but where is the spring sleeping this year!!! We all really need it... :!:
Have nice days!
Roman

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

For Bailout there should be a Speed limit set to avoid stall.
if gpsSpeed is higher than xxx cm/sec its ok to climb.
else only accelerate strait forward.

rbirdie001
Posts: 178
Joined: Fri Apr 01, 2011 10:32 pm
Location: Czech Republic, Prague

Re: Airplane mode RTH

Post by rbirdie001 »

PatrikE wrote:For Bailout there should be a Speed limit set to avoid stall.
if gpsSpeed is higher than xxx cm/sec its ok to climb.
else only accelerate strait forward.

Good idea but risky:
If bailout is used as autolaunch, you launch usually against wind. My plane minimal is about 20km/h, if the wind is 25km/h, GPS will try speed up until ground speed 20km/h which is true airspeed 45km/h. I'm afraid that my weak engine (thrust 60% of plane weight) may need long time to reach that speed so it will fly long time in height of launch (~2m) and can crash into a bush etc...
In the other situation when RTH is activated while flying in the wind direction, automatic will read ground speed 45km/h (20 airspeed + 25 wind speed) so will climb withou worrying the minimal speed and stall the plane... :(
I think that using elevator angle/throttle curve is safer... Once you tune it for your plane, it should work independent to the wind.
Estimation of wind size and it's direction from ground speed difference could work but:
1) It will need some time to calibrate, won't work at launch
2) It will be difficult to compare speeds because of possibly different flight regime (throttle, sticks...)
This means that I'm afraid it can be risky too.
Because of my overspeed problems with the previous version I had similar idea to inhibit descending when speed is over maximal but for the same reason I postponed it...
Additional question: If my engine thrust is about 60% of plane weight, do you think that default

Code: Select all

#define SCALER_THROTTLE 8
is a good start value?
Thanks!
Roman

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

OK.
Until we have a True Airspeed Let's asume that speed is ok when we run at ClimbThrottle.
That's the case in current ver.

SCALER_THROTTLE is multipied with Alterror as a P-term.
50 meter Alterror will give ( 50 * Scaler ) + CruiseThrottle.

My plane is also underpowerd and i use 10 as scaler.
You have to experiment with it a bit.
A higher value will give more power to climb.
Better to start high scaler and lower if it feels to powerful and nervous at small Alterrors.

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

@rbirdie001
(I set the 2 state switch to 0=ARM and 1=RTH only, I guess that this you ment and you just made mistake describing it...)

No it was not a mistake AUX1 is ARM in mid and low position (both checkboxes) but only in low is RTH otherwise MWII would disarm and activate RTH :?: :!:
About speed: normal RC flying is done based on TH position and perceived angle of attack, glider pilots (RC and real ones) are trained to fly just by observing angle of attack of the plane (we are all doing that now). I'm pretty much convinced that most of our flying/navigation can be done with basic knowledge of TH vs. angle of attack and just by tuning of their relationship.

Regards
Andrej

rbirdie001
Posts: 178
Joined: Fri Apr 01, 2011 10:32 pm
Location: Czech Republic, Prague

Re: Airplane mode RTH

Post by rbirdie001 »

crashlander wrote:No it was not a mistake AUX1 is ARM in mid and low position (both checkboxes) but only in low is RTH otherwise MWII would disarm and activate RTH :?: :!:
About speed: normal RC flying is done based on TH position and perceived angle of attack, glider pilots (RC and real ones) are trained to fly just by observing angle of attack of the plane (we are all doing that now). I'm pretty much convinced that most of our flying/navigation can be done with basic knowledge of TH vs. angle of attack and just by tuning of their relationship.

Hi Andrej,
I once more checked it and for me seems to be working config: AUX1 LOW is ARM and AUX1 HIGH is RTH. I think that if ARM would be checked in both positions, MWC couldn't disarm at all.
If I move AUX1 to HIGH (RTH), MWC is still armed while THRO is higher than ZERO and only disarm when THRO is moved all the way down. Not flight tested yet so ideas preventing crash are wellcome... ;)
What do you mean "angle of attack"? Do you mean plane PITCH angle?
Regards
Roman

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Airplane mode RTH

Post by crashlander »

In my case ARM does not mean motor run since I'm using

Code: Select all

#define MOTOR_STOP
and thus I can ARM and motor will only start when TH is higher than zero!
But in RTH mode MWII disarms (and stops motor) if

Code: Select all

#define FAILSAFE_RTH true
when home is reached and than only turning off RTH rearms it.
In this case I also recomend to use

Code: Select all

#define DONT_RESET_HOME_AT_ARM

or your home point will move each time you disable RTH (and RTH disarm was active).
By angle of attack I meant plane pitch angle yes.

Regards Andrej

rbirdie001
Posts: 178
Joined: Fri Apr 01, 2011 10:32 pm
Location: Czech Republic, Prague

Re: Airplane mode RTH

Post by rbirdie001 »

Yes Andrej,
I'm using the same settings

Code: Select all

MOTOR_STOP
FAILSAFE_RTH true
DONT_RESET_HOME_AT_ARM
- that's logical for an airplane.
When I wrote "disarm when THRO is moved all the way down" I really ment "disarm", not stopped engine. You can check, that if a switch is assigned for ARM, complete condition for change state DISARM/ARM and opposit is also THRO at the bottom. This means that once MWC is ARMED and THRO > 0 (i.e. "in flight"), MWC remains armed even if the ARM switch is already deactivated until either THRO=0 or (probably) HOME @ FAILSAFE is reached.
I still think that basically you use the same settings and we just don't understand each other ;)
For sure: on two state (LOW, HIGH) switch you have ARM at LOW, MID is useless because there is never such state and at HIGH you have RTH or RTH+ARM? If RTH+ARM, does your plane disarm when reaches home?
Could you also explain in other words what did you mean "angle of attack"?
Nice evening!
Roman

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Airplane mode RTH

Post by PatrikE »

Code: Select all

#define DONT_RESET_HOME_AT_ARM

To prevent that Home pos is reset i made a change.
In latest ver it's preselected in def.h if any of the planes is selected.
No need to set it in config.
To reset Home calibrate gyro with StickCommand or reboot Arduino.

Code: Select all

#define MOTOR_STOP

I usually lower minthrottle in gui to make motor stop.
And set maxthrottle to 2000 to get Full thottle.

Post Reply