RTH as a failsafe option

[Imweazjac]

WARNING!!! I am not a programmer what so ever, so dont try this at home.
I've been pondering the idea of the RTH as a failsafe option within the multiwii environment because my stupid tx/rx pair doesn't seem to be able to do it and I'm to cheap to buy something else right now. I've been looking through the code and I think I found a way. Here is where I tracked the code.

// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAFE_DELAY is always false
#if ACC
if ( rcOptions[BOXANGLE] || (failsafeCnt > 5*FAILSAFE_DELAY) ) {
// bumpless transfer to Level mode
//if (!f.ANGLE_MODE) {
// errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
//f.ANGLE_MODE = 1;
init_RTH();
//}
} else {
// failsafe support
f.ANGLE_MODE = 0;
f.GPS_BARO_MODE = false;


It seems to work on a bench test. All I did was call the RTH function while commenting out the default angle function. One thing that was little puzzling was GPSHOME didn't light up, Baro did, I'm assuming it was because I was inside and didn't have a GPS signal. I also had to turn of the Baro if it wasn't in the failsafe state because it wouldnt turn off for some reason and kept the motors spinning. Can someone tell me if I'm on to something or am I going to watch my quad fly off into the sunset, or go down in a ball of flames? I still have alot code studying to do before I actually upload and try any of this so again DONT TRY THIS AT HOME I AM NOT A PROGRAMMER!!!!

[Imweazjac]

Scratch that, this wont work. The failsafe throttle speed and timer setting will bring it down. Back to the drawing board.

[Deet]

just do it as radio settings

Assuming you radio is capable of storing failsafe to all channels (some only do throttle)
The store the following as your radio failsafe
Roll/Pitch/Yaw, to Centre
Throttle to Centre
And you AUX channel that selects RTH to that position

Just make sure that you have BARO and MAG selected by that AUX switch with RTH

you can see on the GUI if it is working, do a props off test, and once armed, turn your radio off, you will quickly see if it worked or not

AND NO CODE REQUIRED, not even the failsafe define in the code

[Deet]

just do it as radio settings

Assuming you radio is capable of storing failsafe to all channels (some only do throttle)
The store the following as your radio failsafe
Roll/Pitch/Yaw, to Centre
Throttle to Centre
And you AUX channel that selects RTH to that position

Just make sure that you have BARO and MAG selected by that AUX switch with RTH

you can see on the GUI if it is working, do a props off test, and once armed, turn your radio off, you will quickly see if it worked or not

AND NO CODE REQUIRED, not even the failsafe define in the code

[Imweazjac]

I have a turnigy 9x. I dont think they are able to trigger a toggle as a failsafe, at least I haven't found it in the thousand or so wegpages I've visited. I can however buy a tx module and rx to attach to it and probably will eventually, but modifying the code will only help me learn it, so why not.

[LVNeptune]

@Deet I am new but I don't think that would work in the situation where your Tx can't communicate anymore. If it's handled on the FC you could shut your Tx off and watch it float back to you.

[Imweazjac]

Here is an update. I replace the failsafe functions with the RTH function. Everytime i cut of the receiver the Baro light comes on so I'm thinking that means it's working, the only problem is the motors don't run up as would be expected as it's suppose to climb to 25 meters before it trys to come home , but that could be because I do not have a GPS signal in the house I guess. I do know that the gyros are working as I can pick up the quad and tilt and make the motors come on.

// Failsafe routine - added by MIS
#if defined(FAILSAFE)
if ( failsafeCnt > (5*FAILSAFE_DELAY))&& f.ARMED) { // Stabilize, and set Throttle to specified level
init_RTH();

}
failsafeEvents++;

failsafeCnt++;
#endif

Here is the RTH function that explains why the Barometer light kept coming on when I cut the receiver off.

void init_RTH()
{
f.GPS_mode = GPS_MODE_RTH; // Set GPS_mode to RTH
f.GPS_BARO_MODE = true;
#if defined (I2C_GPS)
GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0); //waypoint zero
#else
GPS_hold[LAT] = GPS_coord[LAT]; //All RTH starts with a poshold
GPS_hold[LON] = GPS_coord[LON]; //This allows to raise to rth altitude
GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON], &GPS_hold[LAT], &GPS_hold[LON]);
NAV_paused_at = 0;
if (GPS_conf.rth_altitude == 0) set_new_altitude(alt.EstAlt); //Return at actual altitude
else { // RTH altitude is defined, but we use it only if we are below it
if (alt.EstAlt < GPS_conf.rth_altitude * 100)
set_new_altitude(GPS_conf.rth_altitude * 100);
else set_new_altitude(alt.EstAlt);
}
f.GPS_head_set = 0; //Allow the RTH ti handle heading
NAV_state = NAV_STATE_RTH_START; //NAV engine status is Starting RTH.
#endif
}

I guess an if else statement would be required in case something is wrong with the GPS, so that it would enter the normal Failsafe routine. Does anyone see a problem with this?

[EOSBandi]

THere are two basic types of faisafe receivers.
With the sophisticated ones such as Frsky, Dragonlink etc. you can preset values for each channel, and the receiver will set these values when lost communication.
The low end tx/rx systems does not have these functions, these systems will output invalid or no signal at the RX side when communication is lost.

The code above is dangerous and not complete. You have to handle situations when you regain control, and also when you does not have gps signal.....

[Imweazjac]

My tx/rx failsafe does not work, I've read somewhere that I can order a Frski in order to get the RTH as failsafe, but that is not the point. This problem can be fixed using software and not having to buy more hardware, because after all frsky is working because they have the correct software installed on their cpu's. I dont recommend anyone run the code above as it is incomplete, but I did not write any code for it, all I did was change which function was called. Here is the original failsafe function that everyone is using.

#if defined(FAILSAFE)
if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED) { // Stabilize, and set Throttle to specified level
for(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = conf.failsafe_throttle;
if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)
go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;
}
if ( failsafeCnt > (5*FAILSAFE_DELAY) && !f.ARMED) { //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeCnt++;
#endif
// end of failsafe routine - next change is made with RcOptions setting




And the modified function.



#if defined(FAILSAFE)
if ( failsafeCnt > (5*FAILSAFE_DELAY))&& f.ARMED) {
init_RTH();

}
failsafeEvents++;

failsafeCnt++;
#endif

I m not a programmer ,but it looks like the situations that need handled are handled the same way the the current failsafe handles the situations.
The point is RTH can be implemented as a failsafe option quite easily. New hardware should not be the only option, that's a cop out.


DO NOT TRY THIS CODE...DANGER!!!...I AM NOT A PROGRAMMER...YOU WILL PROBABLY LOSE YOUR QUAD..........

[Imweazjac]

New code that uses if statement to determine if a gps home setting has been stored and if it has switched to RTH if not the original failsafe is triggered. THIS CODE IS JUST THEORY AND HAS NOT BEEN TESTED!!!!


// Failsafe routine - added by MIS
#if defined(FAILSAFE)
if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED && f.GPS_FIX_HOME) // Stabilize, and set Throttle to specified level
{
init_RTH();
failsafeEvents++;

}
else
{
if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED)
{
for(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = conf.failsafe_throttle;
if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)
go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;

if ( failsafeCnt > (5*FAILSAFE_DELAY) && !f.ARMED) { //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
}
}
failsafeCnt++;


#endif

[Imweazjac]

The final code. It covers making sure GPS is established, GPS home coordinants are established, A secondary failsafe is established, making sure that when the transmitter reconnects the quad should go into a position hold and clears the waypoints.
TRY THIS AT YOUR OWN RISK!!!

#if defined(FAILSAFE)
if (failsafeCnt < (5*FAILSAFE_DELAY) && (ClearNav > 1))
{
GPS_hold[LAT] = GPS_coord[LAT]; // position hold when transmitter reconnects to give operator time
GPS_hold[LON] = GPS_coord[LON]; // to take control
delay(5000);
GPS_reset_nav (); //Clear waypoints if reconnected to transmitter
ClearNav = 0; //Reset counter
}

if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED && f.GPS_FIX_HOME && f.GPS_FIX ) // Stabilize, and set Throttle to specified level
{

init_RTH(); // initiate Return to home

ClearNav++; //counter used to clear waypoints
}
else
{
if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED)
{
for(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = conf.failsafe_throttle;
if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)
go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;

if ( failsafeCnt > (5*FAILSAFE_DELAY) && !f.ARMED) { //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
}
}
failsafeCnt++;


#endif

[phantom8]

@Imweazjac, did you try your failsafe rth code? Does it work? I am in the same boat as you. My receiver doesn't allow preset failsafe values.

[visper]

Are there any results? I want it too.

[fgrs]

same here would love to have RTH as a failsafe

[carlonb]

The final code. It covers making sure GPS is established, GPS home coordinants are established, A secondary failsafe is established, making sure that when the transmitter reconnects the quad should go into a position hold and clears the waypoints.
TRY THIS AT YOUR OWN RISK!!!

#if defined(FAILSAFE)
if (failsafeCnt < (5*FAILSAFE_DELAY) && (ClearNav > 1))
{
GPS_hold[LAT] = GPS_coord[LAT]; // position hold when transmitter reconnects to give operator time
GPS_hold[LON] = GPS_coord[LON]; // to take control
delay(5000);
GPS_reset_nav (); //Clear waypoints if reconnected to transmitter
ClearNav = 0; //Reset counter
}

if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED && f.GPS_FIX_HOME && f.GPS_FIX ) // Stabilize, and set Throttle to specified level
....
....
....
#endif

Hi Imweazjac,
have you already fly tested this solution ?
I'm looking at the code you mod, but i do not understand the "delay(5000);" statement here, I think the cpu during this waiting time (5 secs) can do nothing, so the copter will fall down. Explain better please.

One more detail,
The first statement "if (failsafeCnt < (5*FAILSAFE_DELAY) && (ClearNav > 1)), well, ..... you use ClearNav >1, I think that ClearNav >0 is better.

Anyway, the concept may be interesting, I will look deeply and test it.
Bye