src/main/cms/cms_menu_gps_rescue.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <ctype.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_CMS_GPS_RESCUE_MENU
  29
  30 #include "cli/settings.h"
  31
  32 #include "cms/cms.h"
  33 #include "cms/cms_types.h"
  34 #include "cms/cms_menu_gps_rescue.h"
  35
  36 #include "config/feature.h"
  37
  38 #include "config/config.h"
  39
  40 #include "flight/gps_rescue.h"
  41
  42
  43 static uint16_t gpsRescueConfig_angle; //degrees
  44 static uint16_t gpsRescueConfig_initialAltitudeM; //meters
  45 static uint16_t gpsRescueConfig_descentDistanceM; //meters
  46 static uint16_t gpsRescueConfig_rescueGroundspeed; // centimeters per second
  47 static uint16_t gpsRescueConfig_throttleMin;
  48 static uint16_t gpsRescueConfig_throttleMax;
  49 static uint16_t gpsRescueConfig_throttleHover;
  50 static uint8_t gpsRescueConfig_minSats;
  51 static uint16_t gpsRescueConfig_minRescueDth; //meters
  52 static uint8_t gpsRescueConfig_allowArmingWithoutFix;
  53 static uint16_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD;
  54 static uint16_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
  55 static uint16_t gpsRescueConfig_yawP;
  56 static uint16_t gpsRescueConfig_targetLandingAltitudeM;
  57 static uint16_t gpsRescueConfig_targetLandingDistanceM;
  58 static uint8_t gpsRescueConfig_altitudeMode;
  59 static uint16_t gpsRescueConfig_ascendRate;
  60 static uint16_t gpsRescueConfig_descendRate;
  61
  62 static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
  63 {
  64     UNUSED(pDisp);
  65
  66     gpsRescueConfig_throttleP = gpsRescueConfig()->throttleP;
  67     gpsRescueConfig_throttleI = gpsRescueConfig()->throttleI;
  68     gpsRescueConfig_throttleD = gpsRescueConfig()->throttleD;
  69
  70     gpsRescueConfig_yawP = gpsRescueConfig()->yawP;
  71
  72     gpsRescueConfig_velP = gpsRescueConfig()->velP;
  73     gpsRescueConfig_velI = gpsRescueConfig()->velI;
  74     gpsRescueConfig_velD = gpsRescueConfig()->velD;
  75
  76     return NULL;
  77 }
  78
  79 static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_Entry *self)
  80 {
  81     UNUSED(pDisp);
  82     UNUSED(self);
  83
  84     gpsRescueConfigMutable()->throttleP = gpsRescueConfig_throttleP;
  85     gpsRescueConfigMutable()->throttleI = gpsRescueConfig_throttleI;
  86     gpsRescueConfigMutable()->throttleD = gpsRescueConfig_throttleD;
  87
  88     gpsRescueConfigMutable()->yawP = gpsRescueConfig_yawP;
  89
  90     gpsRescueConfigMutable()->velP = gpsRescueConfig_velP;
  91     gpsRescueConfigMutable()->velI = gpsRescueConfig_velI;
  92     gpsRescueConfigMutable()->velD = gpsRescueConfig_velD;
  93
  94     return NULL;
  95 }
  96
  97 const OSD_Entry cms_menuGpsRescuePidEntries[] =
  98 {
  99     {"--- GPS RESCUE PID---", OME_Label, NULL, NULL},
 100
 101     { "THROTTLE P",        OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleP, 0, 500, 1 } },
 102     { "THROTTLE I",        OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleI, 0, 500, 1 } },
 103     { "THROTTLE D",        OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleD, 0, 500, 1 } },
 104
 105     { "YAW P",             OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_yawP, 0, 500, 1 } },
 106
 107     { "VELOCITY P",        OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velP, 0, 500, 1 } },
 108     { "VELOCITY I",        OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velI, 0, 500, 1 } },
 109     { "VELOCITY D",        OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velD, 0, 500, 1 } },
 110
 111     {"BACK", OME_Back, NULL, NULL},
 112     {NULL, OME_END, NULL, NULL}
 113 };
 114
 115 CMS_Menu cms_menuGpsRescuePid = {
 116 #ifdef CMS_MENU_DEBUG
 117     .GUARD_text = "MENUGPSRPID",
 118     .GUARD_type = OME_MENU,
 119 #endif
 120     .onEnter = cms_menuGpsRescuePidOnEnter,
 121     .onExit = cms_menuGpsRescuePidOnExit,
 122     .onDisplayUpdate = NULL,
 123     .entries = cms_menuGpsRescuePidEntries,
 124 };
 125
 126 static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
 127 {
 128     UNUSED(pDisp);
 129
 130     gpsRescueConfig_angle = gpsRescueConfig()->angle;
 131     gpsRescueConfig_initialAltitudeM = gpsRescueConfig()->initialAltitudeM;
 132     gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
 133     gpsRescueConfig_rescueGroundspeed = gpsRescueConfig()->rescueGroundspeed;
 134     gpsRescueConfig_throttleMin = gpsRescueConfig()->throttleMin  ;
 135     gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax;
 136     gpsRescueConfig_throttleHover = gpsRescueConfig()->throttleHover;
 137     gpsRescueConfig_minSats = gpsRescueConfig()->minSats;
 138     gpsRescueConfig_minRescueDth = gpsRescueConfig()->minRescueDth;
 139     gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix;
 140     gpsRescueConfig_targetLandingDistanceM = gpsRescueConfig()->targetLandingDistanceM;
 141     gpsRescueConfig_targetLandingAltitudeM = gpsRescueConfig()->targetLandingAltitudeM;
 142     gpsRescueConfig_altitudeMode = gpsRescueConfig()->altitudeMode;
 143     gpsRescueConfig_ascendRate = gpsRescueConfig()->ascendRate;
 144     gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
 145
 146     return NULL;
 147 }
 148
 149 static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 150 {
 151     UNUSED(pDisp);
 152     UNUSED(self);
 153
 154     gpsRescueConfigMutable()->angle = gpsRescueConfig_angle;
 155     gpsRescueConfigMutable()->initialAltitudeM = gpsRescueConfig_initialAltitudeM;
 156     gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
 157     gpsRescueConfigMutable()->rescueGroundspeed = gpsRescueConfig_rescueGroundspeed;
 158     gpsRescueConfigMutable()->throttleMin = gpsRescueConfig_throttleMin;
 159     gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax;
 160     gpsRescueConfigMutable()->throttleHover = gpsRescueConfig_throttleHover;
 161     gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats;
 162     gpsRescueConfigMutable()->minRescueDth = gpsRescueConfig_minRescueDth;
 163     gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix;
 164     gpsRescueConfigMutable()->targetLandingDistanceM = gpsRescueConfig_targetLandingDistanceM;
 165     gpsRescueConfigMutable()->targetLandingAltitudeM = gpsRescueConfig_targetLandingAltitudeM;
 166     gpsRescueConfigMutable()->altitudeMode = gpsRescueConfig_altitudeMode;
 167     gpsRescueConfigMutable()->ascendRate = gpsRescueConfig_ascendRate;
 168     gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;
 169
 170     return NULL;
 171 }
 172
 173 const OSD_Entry cmsx_menuGpsRescueEntries[] =
 174 {
 175     {"--- GPS RESCUE ---", OME_Label, NULL, NULL},
 176
 177     { "ANGLE",             OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_angle, 0, 200 ,1 } },
 178     { "MIN DIST HOME   M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minRescueDth, 50, 1000 ,1 } },
 179     { "INITAL ALT      M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialAltitudeM, 20, 100, 1 } },
 180     { "ALTITUDE MODE"    , OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &gpsRescueConfig_altitudeMode, 2, lookupTableRescueAltitudeMode} },
 181     { "DESCENT DIST    M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 30, 500, 1 } },
 182     { "LANDING ALT     M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_targetLandingAltitudeM, 3, 10, 1 } },
 183     { "LANDING DIST    M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_targetLandingDistanceM, 5, 15, 1 } },
 184     { "GROUND SPEED CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_rescueGroundspeed, 30, 3000, 1 } },
 185     { "THROTTLE MIN",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMin, 1000, 2000, 1 } },
 186     { "THROTTLE MAX",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMax, 1000, 2000, 1 } },
 187     { "THROTTLE HOV",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleHover, 1000, 2000, 1 } },
 188     { "ASCEND RATE",       OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_ascendRate, 100, 2500, 1 } },
 189     { "DESCEND RATE",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 100, 500, 1 } },
 190     { "ARM WITHOUT FIX",   OME_Bool | REBOOT_REQUIRED,  NULL, &gpsRescueConfig_allowArmingWithoutFix },
 191     { "MIN SATELITES",     OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } },
 192     { "GPS RESCUE PID",    OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid},
 193
 194     {"BACK", OME_Back, NULL, NULL},
 195     {NULL, OME_END, NULL, NULL}
 196 };
 197
 198 CMS_Menu cmsx_menuGpsRescue = {
 199 #ifdef CMS_MENU_DEBUG
 200     .GUARD_text = "MENUGPSRES",
 201     .GUARD_type = OME_MENU,
 202 #endif
 203     .onEnter = cmsx_menuGpsRescueOnEnter,
 204     .onExit = cmsx_menuGpsRescueOnExit,
 205     .onDisplayUpdate = NULL,
 206     .entries = cmsx_menuGpsRescueEntries,
 207 };
 208
 209 #endif