nhadrian wrote:Hi All,
from now on, the latest revision will be linked into the first comment to see clear what is the last.
BR
Adrian
Hi Adrian,
I have a suggestion, we the quad lost rc signal, can we make it to poshold for a fews seconds, and then start rth?
In my case, iPhone is used as the transmitter, when I switch to map view to send way point, the rc signal thread will be suspended for one or two seconds. as the UI thread on iPhone has the highest priority. when the loading animation is finished, the rc thread resumed. So when the iphone is running the map loading animation, I hope the quad is able to poshold itself and wait for the rc signal to be back.
yes, I can change the failsafe_delay to 2/3 seconds to achieve this, but it is dangerous when I lost my rc signal when the quad is moving with a high speed.
by the way, this is the default failsafe behaviour of DJI naza.
Code: Select all
static struct
{
int16_t events;
uint8_t active; // Failsafe mode status - replace (failsafeCnt > (5*FAILSAFE_DELAY))
#if BARO && defined(FAILSAFE) && (defined(FAILSAFE_ALT_MODE) || defined(FAILSAFE_RTH_MODE))
uint8_t altSet;
#if defined(FAILSAFE_RTH_MODE)
uint8_t confSet;
uint8_t atHome;
uint32_t atHomeDelay;
#if defined(FAILSAFE_RTH_START_DELAY)
uint32_t startDelay;
#endif
#endif
#endif
} failsafe;
Code: Select all
/*
When FAILSAFE_RTH_START_DELAY defined: 0 -> 2 -> 3 -> 1
Else: 0 -> 1
*/
#if defined(FAILSAFE) && defined(FAILSAFE_RTH_MODE) && BARO // set the proper f.modes for RTH function
if (failsafe.active) // after specified guard time after RC signal is lost (in 0.1sec)
{
if (f.ARMED)
{
if (f.GPS_FIX && GPS_numSat >= 5) //check if GPS is ready for RTH
{
switch (failsafe.confSet) //first save the states and prepare to give control to RTH!
{
// poshold
#if defined(FAILSAFE_RTH_START_DELAY)
case 0:
failsafe.confSet = 2;
if (f.GPS_HOME_MODE || !f.GPS_HOLD_MODE) //check if already in RTH or HOLD?
{
f.GPS_HOME_MODE = 0;
f.GPS_HOLD_MODE = 1;
#if defined(AUTOLAND) && defined(RTH_ALT_MODE)
f.AUTOLAND_MODE = 0;
#endif
GPSNavReset = 0;
#if defined(I2C_GPS)
GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD, 0); //waypoint zero
nav_mode = NAV_MODE_POSHOLD;
#else // SERIAL
WP[HOLD].Lat = GPS_coord[LAT];
WP[HOLD].Lon = GPS_coord[LON];
GPS_set_next_wp(&WP[HOLD].Lat, &WP[HOLD].Lon);
nav_mode = NAV_MODE_POSHOLD;
#endif
}
#if defined(FAILSAFE_RTH_DELAY)
failsafe.atHome = 0;
#endif
resetAltHold(); //to be sure that parameters are set to default even if already was in BARO_MODE
break;
#endif // defined(FAILSAFE_RTH_START_DELAY)
// go to home
#if defined(FAILSAFE_RTH_START_DELAY)
case 3:
#else
case 0:
#endif // defined(FAILSAFE_RTH_START_DELAY)
failsafe.confSet = 1;
if (!f.GPS_HOME_MODE || f.GPS_HOLD_MODE) //check if already in RTH or HOLD?
{
f.GPS_HOME_MODE = 1;
f.GPS_HOLD_MODE = 0;
#if defined(AUTOLAND) && defined(RTH_ALT_MODE)
f.AUTOLAND_MODE = 0;
#endif
GPSNavReset = 0;
#if defined(I2C_GPS)
GPS_I2C_command(I2C_GPS_COMMAND_START_NAV, 0); //waypoint zero
nav_mode = NAV_MODE_WP;
#else // SERIAL
GPS_set_next_wp(&WP[HOME].Lat, &WP[HOME].Lon);
nav_mode = NAV_MODE_WP;
#endif
}
#if defined(FAILSAFE_RTH_DELAY)
failsafe.atHome = 0;
#endif
resetAltHold(); //to be sure that parameters are set to default even if already was in BARO_MODE
break;
case 1:
f.GPS_HOME_MODE = 1;
f.GPS_HOLD_MODE = 0;
#if defined(AUTOLAND) && defined(RTH_ALT_MODE)
f.AUTOLAND_MODE = 0;
#endif
break;
#if defined(FAILSAFE_RTH_START_DELAY)
case 2:
f.GPS_HOME_MODE = 0;
f.GPS_HOLD_MODE = 1;
#if defined(AUTOLAND) && defined(RTH_ALT_MODE)
f.AUTOLAND_MODE = 0;
#endif
/*
1. this will not be executed when no gps fix
2. remove it to save power, let's update it when AltVarioChanged changed
*/
/*
if (failsafe.startDelay > FAILSAFE_RTH_START_DELAY * 1000000)
failsafe.confSet = 3;
*/
break;
#endif // defined(FAILSAFE_RTH_START_DELAY)
}
}
}
}
#endif
Code: Select all
#if defined(FAILSAFE_RTH_MODE) && defined(FAILSAFE) && defined(FAILSAFE_RTH_DELAY)
if (failsafe.atHome)
{
failsafe.atHomeDelay += cycleTime;
}
#if defined(FAILSAFE_RTH_START_DELAY)
if (failsafe.confSet == 2) {
failsafe.startDelay += cycleTime;
}
#endif
#endif
Code: Select all
#if defined(FAILSAFE) && (defined(FAILSAFE_ALT_MODE) || defined(FAILSAFE_RTH_MODE)) //failsafe altitude handling codes
if (failsafe.active)
{
if (!AltVarioChanged) //Allcalculations runs on 25 Hz
{
#if defined(FAILSAFE_RTH_MODE)
if (f.GPS_HOME_MODE
#if defined(FAILSAFE_RTH_DELAY)
&& (failsafe.atHomeDelay < (FAILSAFE_RTH_DELAY * 1000000)) //check if hovering at home position for less time than FAILSAFE_RTH_DELAY - if more, will land in failsafe alt mode
#endif
) //check if GPS is ready for RTH
{
switch (nav_mode)
{
case NAV_MODE_POSHOLD:
altToTarget(FAILSAFE_RTH_HOME, FAILSAFE_RTH_VARIO, alt_target_mode_failsafe_home_altitude);
break;
case NAV_MODE_WP:
altToTarget(FAILSAFE_RTH_ALT, FAILSAFE_RTH_VARIO, alt_target_mode_failsafe_rth_altitude);
break;
}
}
else
{
#if defined(FAILSAFE_RTH_START_DELAY)
if (failsafe.confSet == 2)
{
altHoldCode();
if (failsafe.startDelay > FAILSAFE_RTH_START_DELAY * 1000000)
failsafe.confSet = 3;
}
else
altToFailsafe();
#else // defined(FAILSAFE_RTH_START_DELAY)
altToFailsafe();
#endif // defined(FAILSAFE_RTH_START_DELAY)
}
#else
altToFailsafe();
#endif
}
// calc'd based on rcData
rcCommand[THROTTLE] += BaroPID;
}
#endif
#endif