Anyway, I came up with my own quick hack for temporarily enabling GPS HOLD & climbing to the RTH height before actually RTH'ing.
I was hoping I could get another set of eyes on it in case I might have missed something.
Here goes:
in config.h, add a new define in the GPS section
Code: Select all
#define RTH_MIN_HEIGHT 500 //in cm
in Multiwii.cpp, within the GPS mode processing routines
line ~1040+
Code: Select all
#if GPS
#if defined (RTH_MIN_HEIGHT)
static uint8_t rth_height_holding=0;
#endif
static uint8_t GPSNavReset = 1;
if (f.GPS_FIX && GPS_numSat >= 5 ) {
if (rcOptions[BOXGPSHOME]) { // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority
if (!f.GPS_HOME_MODE) {
#if defined (RTH_MIN_HEIGHT)
if(alt.EstAlt<RTH_MIN_HEIGHT){
if(!rth_height_holding){
rth_height_holding=1;
AltHold=RTH_MIN_HEIGHT;
f.GPS_HOLD_MODE = 1;
GPSNavReset = 0;
#if defined(I2C_GPS)
GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0);
#else
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
#endif
}
}else{
rth_height_holding=0;
#endif
f.GPS_HOME_MODE = 1;
f.GPS_HOLD_MODE = 0;
GPSNavReset = 0;
#if defined(I2C_GPS)
GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0); //waypoint zero
#else // SERIAL
GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]);
nav_mode = NAV_MODE_WP;
#endif
#if defined(RTH_MIN_HEIGHT)
}
#endif
}
} else {
#if defined (RTH_MIN_HEIGHT)
rth_height_holding=0;
#endif
f.GPS_HOME_MODE = 0;
if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL])< AP_MODE && abs(rcCommand[PITCH]) < AP_MODE) {
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPSNavReset = 0;
#if defined(I2C_GPS)
GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0);
#else
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
#endif
}
} else {
f.GPS_HOLD_MODE = 0;
// both boxes are unselected here, nav is reset if not already done
if (GPSNavReset == 0 ) {
GPSNavReset = 1;
GPS_reset_nav();
}
}
}
} else {
#if defined (RTH_MIN_HEIGHT)
rth_height_holding=0;
#endif
f.GPS_HOME_MODE = 0;
f.GPS_HOLD_MODE = 0;
#if !defined(I2C_GPS)
nav_mode = NAV_MODE_NONE;
#endif
}
#endif
Basic idea is to test if the current height is below the RTH height when trying to engage GPS HOME mode. If so, then force GPS HOLD mode and set AltHold to the target height RTH_MIN_HEIGHT (this assumes that the user also already enabled BARO mode).
I don't intend to propose this patch for all users or for integration into the mainstream MW code. I'm also not concerned with the sophistication or ugliness of the code at this point. I'd just like to get this working on my own personal setup, so I'd appreciate some feedback in case there's something that I'm missing (i.e., perhaps there's some kind of weird fall-through condition that I don't see, where my code will fail and cause things to get stuck in a certain mode or completely break RTH). thanks in advance.
