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/position.h"
  41
  42 #include "pg/autopilot.h"
  43 #include "pg/gps_rescue.h"
  44
  45 static uint16_t gpsRescueConfig_minStartDistM; //meters
  46 static uint8_t gpsRescueConfig_altitudeMode;
  47 static uint16_t gpsRescueConfig_initialClimbM; // meters
  48 static uint16_t gpsRescueConfig_ascendRate;
  49
  50 static uint16_t gpsRescueConfig_returnAltitudeM; //meters
  51 static uint16_t gpsRescueConfig_groundSpeedCmS; // centimeters per second
  52 static uint8_t gpsRescueConfig_angle; //degrees
  53
  54 static uint16_t gpsRescueConfig_descentDistanceM; //meters
  55 static uint16_t gpsRescueConfig_descendRate;
  56 static uint8_t apConfig_landingAltitudeM;
  57
  58 static uint16_t apConfig_throttleMin;
  59 static uint16_t apConfig_throttleMax;
  60 static uint16_t apConfig_hoverThrottle;
  61
  62 static uint8_t gpsRescueConfig_minSats;
  63 static uint8_t gpsRescueConfig_allowArmingWithoutFix;
  64
  65 static uint8_t apConfig_altitude_P, apConfig_altitude_I, apConfig_altitude_D, apConfig_altitude_F;
  66 static uint8_t gpsRescueConfig_yawP;
  67 static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
  68
  69 static uint8_t gpsRescueConfig_pitchCutoffHz;
  70 static uint8_t gpsRescueConfig_imuYawGain;
  71
  72 static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
  73 {
  74     UNUSED(pDisp);
  75
  76     apConfig_altitude_P = apConfig()->altitude_P;
  77     apConfig_altitude_I = apConfig()->altitude_I;
  78     apConfig_altitude_D = apConfig()->altitude_D;
  79     apConfig_altitude_F = apConfig()->altitude_F;
  80
  81     gpsRescueConfig_yawP = gpsRescueConfig()->yawP;
  82
  83     gpsRescueConfig_velP = gpsRescueConfig()->velP;
  84     gpsRescueConfig_velI = gpsRescueConfig()->velI;
  85     gpsRescueConfig_velD = gpsRescueConfig()->velD;
  86
  87     gpsRescueConfig_pitchCutoffHz = gpsRescueConfig()->pitchCutoffHz;
  88     gpsRescueConfig_imuYawGain = gpsRescueConfig()->imuYawGain;
  89
  90     return NULL;
  91 }
  92
  93 static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_Entry *self)
  94 {
  95     UNUSED(pDisp);
  96     UNUSED(self);
  97
  98     apConfigMutable()->altitude_P = apConfig_altitude_P;
  99     apConfigMutable()->altitude_I = apConfig_altitude_I;
 100     apConfigMutable()->altitude_D = apConfig_altitude_D;
 101     apConfigMutable()->altitude_F = apConfig_altitude_F;
 102
 103     gpsRescueConfigMutable()->yawP = gpsRescueConfig_yawP;
 104
 105     gpsRescueConfigMutable()->velP = gpsRescueConfig_velP;
 106     gpsRescueConfigMutable()->velI = gpsRescueConfig_velI;
 107     gpsRescueConfigMutable()->velD = gpsRescueConfig_velD;
 108
 109     gpsRescueConfigMutable()->pitchCutoffHz = gpsRescueConfig_pitchCutoffHz;
 110     gpsRescueConfigMutable()->imuYawGain = gpsRescueConfig_imuYawGain;
 111
 112     return NULL;
 113 }
 114
 115 const OSD_Entry cms_menuGpsRescuePidEntries[] =
 116 {
 117     {"--- GPS RESCUE PID---", OME_Label, NULL, NULL},
 118
 119     { "ALTITUDE P",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_P, 0, 200, 1 } },
 120     { "ALTITUDE I",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_I, 0, 200, 1 } },
 121     { "ALTITUDE D",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_D, 0, 200, 1 } },
 122     { "ALTITUDE F",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_F, 0, 200, 1 } },
 123
 124     { "YAW P",             OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_yawP, 0, 200, 1 } },
 125
 126     { "VELOCITY P",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velP, 0, 200, 1 } },
 127     { "VELOCITY I",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velI, 0, 200, 1 } },
 128     { "VELOCITY D",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velD, 0, 200, 1 } },
 129
 130     { "SMOOTHING",         OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_pitchCutoffHz, 10, 255, 1 } },
 131     { "IMU_YAW_GAIN",      OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_imuYawGain, 5, 20, 1 } },
 132
 133     {"BACK", OME_Back, NULL, NULL},
 134     {NULL, OME_END, NULL, NULL}
 135 };
 136
 137 CMS_Menu cms_menuGpsRescuePid = {
 138 #ifdef CMS_MENU_DEBUG
 139     .GUARD_text = "MENUGPSRPID",
 140     .GUARD_type = OME_MENU,
 141 #endif
 142     .onEnter = cms_menuGpsRescuePidOnEnter,
 143     .onExit = cms_menuGpsRescuePidOnExit,
 144     .onDisplayUpdate = NULL,
 145     .entries = cms_menuGpsRescuePidEntries,
 146 };
 147
 148 static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
 149 {
 150     UNUSED(pDisp);
 151
 152     gpsRescueConfig_minStartDistM = gpsRescueConfig()->minStartDistM;
 153     gpsRescueConfig_altitudeMode = gpsRescueConfig()->altitudeMode;
 154     gpsRescueConfig_initialClimbM = gpsRescueConfig()->initialClimbM;
 155     gpsRescueConfig_ascendRate = gpsRescueConfig()->ascendRate;
 156
 157     gpsRescueConfig_returnAltitudeM = gpsRescueConfig()->returnAltitudeM;
 158     gpsRescueConfig_groundSpeedCmS = gpsRescueConfig()->groundSpeedCmS;
 159     gpsRescueConfig_angle = gpsRescueConfig()->maxRescueAngle;
 160
 161     gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
 162     gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
 163     apConfig_landingAltitudeM = apConfig()->landing_altitude_m;
 164
 165     apConfig_throttleMin = apConfig()->throttle_min;
 166     apConfig_throttleMax = apConfig()->throttle_max;
 167     apConfig_hoverThrottle = apConfig()->hover_throttle;
 168
 169     gpsRescueConfig_minSats = gpsRescueConfig()->minSats;
 170     gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix;
 171
 172     return NULL;
 173 }
 174
 175 static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 176 {
 177     UNUSED(pDisp);
 178     UNUSED(self);
 179
 180     gpsRescueConfigMutable()->minStartDistM = gpsRescueConfig_minStartDistM;
 181     gpsRescueConfigMutable()->altitudeMode = gpsRescueConfig_altitudeMode;
 182     gpsRescueConfigMutable()->initialClimbM = gpsRescueConfig_initialClimbM;
 183     gpsRescueConfigMutable()->ascendRate = gpsRescueConfig_ascendRate;
 184
 185     gpsRescueConfigMutable()->returnAltitudeM = gpsRescueConfig_returnAltitudeM;
 186     gpsRescueConfigMutable()->groundSpeedCmS = gpsRescueConfig_groundSpeedCmS;
 187     gpsRescueConfigMutable()->maxRescueAngle = gpsRescueConfig_angle;
 188
 189     gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
 190     gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;
 191     apConfigMutable()->landing_altitude_m = apConfig_landingAltitudeM;
 192
 193     apConfigMutable()->throttle_min = apConfig_throttleMin;
 194     apConfigMutable()->throttle_max = apConfig_throttleMax;
 195     apConfigMutable()->hover_throttle = apConfig_hoverThrottle;
 196
 197     gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats;
 198     gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix;
 199
 200     return NULL;
 201 }
 202
 203 const OSD_Entry cmsx_menuGpsRescueEntries[] =
 204 {
 205     {"--- GPS RESCUE ---", OME_Label, NULL, NULL},
 206
 207     { "MIN START DIST  M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minStartDistM, 20, 1000, 1 } },
 208     { "ALTITUDE MODE"    , OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &gpsRescueConfig_altitudeMode, 2, lookupTableRescueAltitudeMode} },
 209     { "INITAL CLIMB    M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialClimbM, 0, 100, 1 } },
 210     { "ASCEND RATE  CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_ascendRate, 50, 2500, 1 } },
 211
 212     { "RETURN ALT      M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_returnAltitudeM, 2, 255, 1 } },
 213     { "RETURN SPEED CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_groundSpeedCmS, 0, 3000, 1 } },
 214     { "PITCH ANGLE MAX",   OME_UINT8  | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t) { &gpsRescueConfig_angle, 0, 60, 1 } },
 215
 216     { "DESCENT DIST    M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 5, 500, 1 } },
 217     { "DESCENT RATE CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 25, 500, 1 } },
 218     { "LANDING ALT     M", OME_UINT8  | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t) { &apConfig_landingAltitudeM, 1, 15, 1 } },
 219
 220     { "THROTTLE MIN",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_throttleMin, 1050, 1400, 1 } },
 221     { "THROTTLE MAX",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_throttleMax, 1400, 2000, 1 } },
 222     { "THROTTLE HOV",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_hoverThrottle, 1100, 1700, 1 } },
 223
 224     { "SATS REQUIRED",     OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } },
 225     { "ARM WITHOUT FIX",   OME_Bool | REBOOT_REQUIRED,  NULL, &gpsRescueConfig_allowArmingWithoutFix },
 226
 227     { "GPS RESCUE PID",    OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid},
 228
 229     {"BACK", OME_Back, NULL, NULL},
 230     {NULL, OME_END, NULL, NULL}
 231 };
 232
 233 CMS_Menu cmsx_menuGpsRescue = {
 234 #ifdef CMS_MENU_DEBUG
 235     .GUARD_text = "MENUGPSRES",
 236     .GUARD_type = OME_MENU,
 237 #endif
 238     .onEnter = cmsx_menuGpsRescueOnEnter,
 239     .onExit = cmsx_menuGpsRescueOnExit,
 240     .onDisplayUpdate = NULL,
 241     .entries = cmsx_menuGpsRescueEntries,
 242 };
 243
 244 #endif