Navigation based failsafe

[Solarwater]

One important feature that I think multiwii is missing is Nav based failsafe.

So I have been working on implementing it. Basically I have combined the built in failsafe with the GPS Nav system.
The current implementation I have has 3 new options in the failsafe section of config.h.

FAILSAFE_LAND, performs a navigation based land instead of the usual failsafe throttle cut.
FAILSAFE_RTH, returns to the home position, if failsafe_land is also enabled, it will then perform a land when it reaches home.
FAILSAFE_IGNORE_LAND, this will ignore the failsafe state if a land is already in progress. This way if you are already nav landing, and the radio signal is lost, it will finish the land instead of rth or starting over the land process.

Failsafe engagement times and thresholds are kept from the original failsafe options, and if gps fix is lost or below 5 satellites, it will fail-over to the traditional throttle cut failsafe.

I have attached the modified files that I am running, I have made a couple other small tweaks for my setup, so I am posting more for looking at my implementation rather than simple load and fly (probably best to run a diff). I have done a good amount of simulated testing with the nav gps simulator, and have made several successful flight tests, so I am fairly confident in the code. I tried to comment my code thoroughly, but let me know if you have any questions or suggestions. This is based off version 2.4.

As usual, try at your own risk.

Let me know what you guys think.
Thanks.

[pelotron]

Nice; I've been looking for this feature in MultiWii for a long time too. I will test next time I'm out in the field.

[PatrikE]

It should be possible to use same code i use for Fxedwing navigation code

[Droontje]

Hi Solarwater,

I agree that failsafe should be able to use the nav functions. Thanks for your work on this. I will try and test it.

Question: have you tried during your tests what happens when navivigation to home is engaged and the RC-transmission is re-established?
Please look at my post at http://www.multiwii.com/forum/viewtopic.php?f=8&t=6308. I experienced some weird behaviour.

Beside the functions that you have added, i think that there shoud be a seperate failsafe mode during the engagement in a mission. Not so much for flying BLOS (which is prohibited in most countries anyway), but for the instance where RC connection during a mission could be momentarily lost when the aircraft is flying behind a tree or building. I think failsafe in that instance should let the mission carry on, allso after the RC connection is re-established. And when connection is not back at the end of the mission, preferably land the aircraft (using nav). In a well planned mission this should be near to where the groundstation is, which would be preferable than a landing somewhere along the mission path.
Do you agree with this, and would you consider adding this to your failsafe routine?
And could you confirm that at the moment the software doesn't handle a re-establishment of the RC connection well (resulting in spiral)?

Greetings, Dirk

[Solarwater]

Hey Dirk, I have never seen the spiral effect you mention. My code simply enables the built in NAV functions, and when signal is returned, then the modes are returned to whatever the transmitter is currently set to. So barring any bugs, it is the same as you manually flipping the switch to RTH or LAND. If you test my implementation and find an issue, please let me know.

I had thought about making a failsafe_finish_mission option. I have not played with the missions very much, so I haven't gotten to it. I do think it is a good option to have, so I will take a look at it.

[carlonb]

Hi solarwater,
Good job, I've done some tests with NAV simulator and then real fly tested. It seems run well.
In the next I will do further tests.
Bye
Carlo

[GTP_F]

/* Deleted to avoid confusions with Solarwater's code (which is more robust than mine) */

Thanks,
GTP_F

[Droontje]

Hi GTP_P,
I have done a test with your setup. After switching my RC transmitter off, my quad returned to home as expected, but after that it did not land but continued to fly above the home location.
Any idea why it did'nt land as instructed by the setting in the config?
Greetz, Dirk

[FengShuiDrone]

.

[FengShuiDrone]

.

[Droontje]

Hello,
I did have all three options enabled in the config file. So it should first rth then land. But it remained in 'endless' rth position. I was able to test this once, because soon after that i had a crash and ruined my gps unit. Waiting on delivery of a mee one.

Greetz, Dirk

[FengShuiDrone]

.

[Droontje]

Hello again,
OK, i did not have a waypount with landing in the mission. As you argued, baro information is not very reliable therefor it is better to take control after a mission and land by hand.
But in the case of a failsafe event i would prefer an automated landing (with or without gps assistance) because the transmitter might stay out of service (broken or battery failure).
Why would the failsafe-land need a seperate mission waypoint for that? It can just land at the present (home) location.
After all this is what the old failsafe routine does. Just take the home coordinates would do. In that case the gps assisted failsafe would be perfect in that it brings the bird home and on the ground when the rc fails. As said before it would be preferable if a mission could be continued in case of a (temporary) outage of the rc connection. After all, you are not controlling the machine actively during a mission anyhow.
Greetz, Dirk

[FengShuiDrone]

.

[carlonb]

Hi all,
This post is not strictly related with Failsafe but related to Landing routine.
As you may have verifyed, some time after land the motors do not stops automatically.

Some time ago, I've investigate in it and discovered why. See here http://www.multiwii.com/forum/viewtopic.php?f=8&t=3989&p=55305#p55408

I've mod my MWii 2.4 sketch and tested, all seems run OK, now the motors stops every time.
This is my code mod of the routine "check_land()" in file gps.cpp

Code: Select all

#define LAND_DETECT_THRESHOLD 40      //Counts of land situation
//#define BAROPIDMIN           -180     //BaroPID reach this if we landed..... Carlonb commented
//Check if we landed or not
void check_land() {
int16_t baropidmin;  // Carlonb added
// Carlonb mod: BaroPID is related to Pterm of althold PID, if P=6.4 as per default, OK, BAROPIDMIN=-180 but if P=2.3 the max value of BaroPID will be -112, here will never stops motors.
// So, i've calculated some of these values changing the Pterm and I've found these results and put them in few if...
// The best and elegant coding is runtime pre-calc baropidmin avoiding all these if statements...I'm not able to do it.
  if(conf.pid[PIDALT].P8 >= 60) baropidmin=-190;      // BaroPID with P>=7.0 will be not grather than -209 // carlonb added
  else if(conf.pid[PIDALT].P8 >= 50) baropidmin=-170; // carlonb added
  else if(conf.pid[PIDALT].P8 >= 40) baropidmin=-145;
  else if(conf.pid[PIDALT].P8 >= 30) baropidmin=-125;
  else if(conf.pid[PIDALT].P8 >= 20) baropidmin=-100; 
  else if(conf.pid[PIDALT].P8 >= 10) baropidmin=-80;
  else if(conf.pid[PIDALT].P8 >= 0) baropidmin=-55; 

  // detect whether we have landed by watching for low climb rate and throttle control
//  if ( (abs(alt.vario) < 20) && (BaroPID < BAROPIDMIN)) {  // carlonb commented
  if ( (abs(alt.vario) < 20) && (BaroPID < baropidmin)) { // carlonb added
    if (!f.LAND_COMPLETED) {
      if( land_detect < LAND_DETECT_THRESHOLD) {
        land_detect++;
      } else {
        f.LAND_COMPLETED = 1;
        land_detect = 0;
      }
    }
  } else {
    // we've detected movement up or down so reset land_detector
    land_detect = 0;
    if(f.LAND_COMPLETED) {
      f.LAND_COMPLETED = 0;
    }
  }
}

This mod is not professional coding, but is working.
Could someone test it and suggest other best solutions or improvements ?
Bye
Carlo

[Solarwater]

Hi guys, I am glad my thread is getting some attention.

To clarify, my code does not use mission waypoints at all. You do not need any waypoints set up to use the failsafe. The only requirement is the home position, which by default is set on arming. I have my quad set up to not arm unless it has a 3d fix to help guard against a bad home position.

seen in config.h
#define ONLY_ALLOW_ARM_WITH_GPS_3DFIX // Only allow FC arming if GPS has a 3D fix.


GTP_F's code looks like it takes sections of my code and places it in other locations?, so I don't have knowledge on how it works, so I can not comment on it and I have never used it. Right now, in my code, the failsafe will not let a mission continue. I don't have that as an option since I have spent very little time with the mission side of the NAV code. Right now the failsafe always takes over with the exception of a land in progress (optional).

I have been very busy and haven't been on the forum in several months. So if you have any other questions, I should be a little more responsive.

I would really love if one of the main devs would take a look at my code and consider integrating it into a release.

[FengShuiDrone]

.

[Solarwater]

Excuse me if I misunderstand, but no mission set up is required for a nav land. LAND is a box mode that you can set with an aux switch without being in a mission. If you were to select LAND via an aux switch, it will land in its current 2D location. No mission set up required to use the standalone LAND box mode.

What likely happened, is he or both of you are using GTP_F's code, which I did not make, and he left out an important change in GPS.cpp that allows a RTH to roll into a LAND. This also seems to be a bug that I corrected in my example code, but may have been intentional? (see the changes I made in GPS.cpp)

I did not make the code from GTP_F, he repackaged some code that I had written. I feel that my implementation is a little more robust, so please look at what I attached in the first post. I can not guarantee his code functions at all like mine.

Missions are not utilized for my failsafe, I have tested with a completely wiped FC with no mission ever entered, as well as one with a mission programmed. As of right now, my code completely overrides a mission, so no matter what is in the saved mission (or none at all), it will still RTH, and if set, it will perform a NAV land at the home position. Please don't confuse the NAV system with missions. They are related, but basically missions will call these independent NAV functions according to the mission steps programmed. All the functions are operable without requiring a mission to call them.

[FengShuiDrone]

.

[Solarwater]

Yes I have tested failsafe while running a mission. I have run several HIL and physical tests. The mission is canceled. My code is actually quite simple, it overrides whatever your aux mode switches are set to, and sets the modes to get RTH, LAND, or both. You can only get a land and RTH together if you make my change in GPS.cpp.

If you read my code changes, you will see how that works.

Just to be sure, since I wrote this code and did my testing several months ago, I just loaded up a mission, and tested shutting off the transmitter while the mission was running. As expected, it waited the failsafe time delay, and then proceeded home and landed.

I also just tested a rth/land via switches. Also worked fine. Not sure why you are not having the same results.

Are you sure you are running my code and not GTP_F's?

And technobabble? you are in the software development forum...

I would be happy to look at potential bugs, but I need more condition details. First step being, make sure you are using my code.

[edsimmons3]

Hi Solarwater,

I took a look at your code and really like the additions you have made. I grabbed a fresh fork of MultiWii 2.4 on github and your files and merged them...

The results of this are here: https://github.com/estechnical/multiwii-firmware. I'd be really grateful for any feedback, so far I have bench tested this successfully with the config file provided here: http://pastebin.com/vbPq21gf

I hope to get my own GPS PIDs sorted out soon so I can test this myself too.

Enjoy,
Ed

[edsimmons3]

Hi Solarwater,

I have just added some changes to the code, now it is possible to compile it without GPS support and without FAILSAFE_RTH enabled. Previously there was a check for GPS fix that was was always included in the code even without a GPS enabled, this caused it to fail.

I hope that once tested we can somehow get this merged into the main source code...

Ed

[Solarwater]

Hi edsimmons, thanks for picking up my code. It has been a real long time since I have worked on it, and would have to wrap my head around it again.

I see the issue you are talking about with GPS disabled. I had done no testing with GPS disabled, so I never caught it.

As a note, I saw the diff on your latest commit, in lines 883 and 900, you use "#if GPS && defined(FAILSAFE_FTH)", could you just get away with "#if GPS"? Since the next line checks for a gps failsafe option being enabled.

Thanks for the attention on this, I would really like to see this function added to multiwii.

[edsimmons3]

Hi Solarwater,

Gald you're still about - I guess you're right... the second check seems redundant...

I'll try and get this tested soon and let you know the results. I saw that GPS Home is enabled when I switch off my TX, so that's a good sign Nothing crazy happens either

Have fun.
Ed

[Seb312]

Hi all,

Thank you "Solarwater" for this feature.
I tested your development yesterday on my machine (Multiwii 2.4 with your code) and I can confirm that it works very well.
I think this feature should be integrated into the master.
I tested :
- Switching off the remote only (in manual mode => GPS calibration has already been done) => RTH & Land have been performed;
- Switching off the remote only (horizon) => RTH & Land have been performed;
- Switching off the remote only (horizon + mag + baro) => RTH & Land have been performed;
- Switching off the remote only (horizon + mag + GPS lock) => RTH & Land have been performed;
- Switching off the remote only (horizon + mag + baro + GPS lock) => RTH & Land have been performed;
- Switching off the remote control during a mission => mission has been stopped as expected and RTH & Land have been performed;
- Switching off the remote control during a landing. => No action, the landing has been performed, not RTH & Land from failsafe.

I can test other cases if needed.