mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
Shared altitude control parameters (#13884)
This commit is contained in:
ctzsnooze
GitHub
parent
7156dc84a3
commit
16c157e840
28 changed files with 383 additions and 273 deletions
|
|
@ -21,6 +21,7 @@ PG_SRC = \
|
|||
pg/msp.c \
|
||||
pg/pg.c \
|
||||
pg/piniobox.c \
|
||||
pg/position_control.c \
|
||||
pg/pinio.c \
|
||||
pg/pin_pull_up_down.c \
|
||||
pg/rcdevice.c \
|
||||
|
|
@ -156,6 +157,7 @@ COMMON_SRC = \
|
|||
fc/rc_controls.c \
|
||||
fc/rc_modes.c \
|
||||
flight/position.c \
|
||||
flight/position_control.c \
|
||||
flight/failsafe.c \
|
||||
flight/gps_rescue.c \
|
||||
fc/gps_lap_timer.c \
|
||||
|
|
|
|||
|
|
@ -68,6 +68,7 @@
|
|||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/rpm_filter.h"
|
||||
#include "flight/servos.h"
|
||||
|
||||
|
|
@ -1588,8 +1589,12 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_PREFER_BARO, "%d", positionConfig()->altitude_prefer_baro);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_LPF, "%d", positionConfig()->altitude_lpf);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", positionConfig()->hover_throttle);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", positionConfig()->landing_altitude_m);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", positionControlConfig()->hover_throttle);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", positionControlConfig()->landing_altitude_m);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d", positionControlConfig()->altitude_P);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d", positionControlConfig()->altitude_I);;
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d", positionControlConfig()->altitude_D);;
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d", positionControlConfig()->altitude_F);
|
||||
|
||||
#ifdef USE_MAG
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware);
|
||||
|
|
@ -1688,9 +1693,6 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d", gpsRescueConfig()->minSats)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix)
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_P, "%d", gpsRescueConfig()->throttleP)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_I, "%d", gpsRescueConfig()->throttleI)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_D, "%d", gpsRescueConfig()->throttleD)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_P, "%d", gpsRescueConfig()->velP)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD)
|
||||
|
|
@ -1703,9 +1705,6 @@ static bool blackboxWriteSysinfo(void)
|
|||
#endif // USE_GPS
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_P, "%d", altholdConfig()->alt_hold_pid_p);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_I, "%d", altholdConfig()->alt_hold_pid_i);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_D, "%d", altholdConfig()->alt_hold_pid_d);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_THROTTLE_MIN, "%d", altholdConfig()->alt_hold_throttle_min);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_THROTTLE_MAX, "%d", altholdConfig()->alt_hold_throttle_max);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE, "%d", altholdConfig()->alt_hold_target_adjust_rate);
|
||||
|
|
|
|||
|
|
@ -109,6 +109,7 @@ bool cliMode = false;
|
|||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/servos.h"
|
||||
|
||||
#include "io/asyncfatfs/asyncfatfs.h"
|
||||
|
|
|
|||
|
|
@ -62,6 +62,7 @@
|
|||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/alt_hold.h"
|
||||
#include "flight/rpm_filter.h"
|
||||
#include "flight/servos.h"
|
||||
|
|
@ -1066,9 +1067,6 @@ const clivalue_t valueTable[] = {
|
|||
{ PARAM_NAME_GPS_RESCUE_MIN_SATS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
|
||||
{ PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) },
|
||||
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) },
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) },
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleD) },
|
||||
{ PARAM_NAME_GPS_RESCUE_VELOCITY_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velP) },
|
||||
{ PARAM_NAME_GPS_RESCUE_VELOCITY_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velI) },
|
||||
{ PARAM_NAME_GPS_RESCUE_VELOCITY_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velD) },
|
||||
|
|
@ -1093,10 +1091,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) },
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
{ PARAM_NAME_ALT_HOLD_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_pid_p) },
|
||||
{ PARAM_NAME_ALT_HOLD_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_pid_i) },
|
||||
{ PARAM_NAME_ALT_HOLD_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_pid_d) },
|
||||
|
||||
{ PARAM_NAME_ALT_HOLD_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_throttle_min) },
|
||||
{ PARAM_NAME_ALT_HOLD_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1400, 2000 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_throttle_max) },
|
||||
{ PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_target_adjust_rate) },
|
||||
|
|
@ -1823,8 +1817,14 @@ const clivalue_t valueTable[] = {
|
|||
{ PARAM_NAME_ALTITUDE_PREFER_BARO, VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_POSITION, offsetof(positionConfig_t, altitude_prefer_baro) },
|
||||
{ PARAM_NAME_ALTITUDE_LPF, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_lpf) },
|
||||
{ PARAM_NAME_ALTITUDE_D_LPF, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_d_lpf) },
|
||||
{ PARAM_NAME_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_POSITION, offsetof(positionConfig_t, hover_throttle) },
|
||||
{ PARAM_NAME_LANDING_ALTITUDE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION, offsetof(positionConfig_t, landing_altitude_m) },
|
||||
|
||||
// PG_POSITION_CONTROL
|
||||
{ PARAM_NAME_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, hover_throttle) },
|
||||
{ PARAM_NAME_LANDING_ALTITUDE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, landing_altitude_m) },
|
||||
{ PARAM_NAME_ALTITUDE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_P) },
|
||||
{ PARAM_NAME_ALTITUDE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_I) },
|
||||
{ PARAM_NAME_ALTITUDE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_D) },
|
||||
{ PARAM_NAME_ALTITUDE_F, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_F) },
|
||||
|
||||
// PG_MODE_ACTIVATION_CONFIG
|
||||
#if defined(USE_CUSTOM_BOX_NAMES)
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@
|
|||
|
||||
#include "flight/gps_rescue.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
|
||||
static uint16_t gpsRescueConfig_minStartDistM; //meters
|
||||
static uint8_t gpsRescueConfig_altitudeMode;
|
||||
|
|
@ -51,16 +52,16 @@ static uint8_t gpsRescueConfig_angle; //degrees
|
|||
|
||||
static uint16_t gpsRescueConfig_descentDistanceM; //meters
|
||||
static uint16_t gpsRescueConfig_descendRate;
|
||||
static uint8_t positionConfig_landingAltitudeM;
|
||||
static uint8_t positionControlConfig_landingAltitudeM;
|
||||
|
||||
static uint16_t gpsRescueConfig_throttleMin;
|
||||
static uint16_t gpsRescueConfig_throttleMax;
|
||||
static uint16_t positionConfig_hoverThrottle;
|
||||
static uint16_t positionControlConfig_hoverThrottle;
|
||||
|
||||
static uint8_t gpsRescueConfig_minSats;
|
||||
static uint8_t gpsRescueConfig_allowArmingWithoutFix;
|
||||
|
||||
static uint8_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD;
|
||||
static uint8_t positionControlConfig_altitude_P, positionControlConfig_altitude_I, positionControlConfig_altitude_D, positionControlConfig_altitude_F;
|
||||
static uint8_t gpsRescueConfig_yawP;
|
||||
static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
|
||||
|
||||
|
|
@ -71,9 +72,10 @@ static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
|
|||
{
|
||||
UNUSED(pDisp);
|
||||
|
||||
gpsRescueConfig_throttleP = gpsRescueConfig()->throttleP;
|
||||
gpsRescueConfig_throttleI = gpsRescueConfig()->throttleI;
|
||||
gpsRescueConfig_throttleD = gpsRescueConfig()->throttleD;
|
||||
positionControlConfig_altitude_P = positionControlConfig()->altitude_P;
|
||||
positionControlConfig_altitude_I = positionControlConfig()->altitude_I;
|
||||
positionControlConfig_altitude_D = positionControlConfig()->altitude_D;
|
||||
positionControlConfig_altitude_F = positionControlConfig()->altitude_F;
|
||||
|
||||
gpsRescueConfig_yawP = gpsRescueConfig()->yawP;
|
||||
|
||||
|
|
@ -92,9 +94,10 @@ static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_En
|
|||
UNUSED(pDisp);
|
||||
UNUSED(self);
|
||||
|
||||
gpsRescueConfigMutable()->throttleP = gpsRescueConfig_throttleP;
|
||||
gpsRescueConfigMutable()->throttleI = gpsRescueConfig_throttleI;
|
||||
gpsRescueConfigMutable()->throttleD = gpsRescueConfig_throttleD;
|
||||
positionControlConfigMutable()->altitude_P = positionControlConfig_altitude_P;
|
||||
positionControlConfigMutable()->altitude_I = positionControlConfig_altitude_I;
|
||||
positionControlConfigMutable()->altitude_D = positionControlConfig_altitude_D;
|
||||
positionControlConfigMutable()->altitude_F = positionControlConfig_altitude_F;
|
||||
|
||||
gpsRescueConfigMutable()->yawP = gpsRescueConfig_yawP;
|
||||
|
||||
|
|
@ -112,9 +115,10 @@ const OSD_Entry cms_menuGpsRescuePidEntries[] =
|
|||
{
|
||||
{"--- GPS RESCUE PID---", OME_Label, NULL, NULL},
|
||||
|
||||
{ "THROTTLE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_throttleP, 0, 255, 1 } },
|
||||
{ "THROTTLE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_throttleI, 0, 255, 1 } },
|
||||
{ "THROTTLE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_throttleD, 0, 255, 1 } },
|
||||
{ "ALTITUDE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_P, 0, 255, 1 } },
|
||||
{ "ALTITUDE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_I, 0, 255, 1 } },
|
||||
{ "ALTITUDE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_D, 0, 255, 1 } },
|
||||
{ "ALTITUDE F", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_F, 0, 255, 1 } },
|
||||
|
||||
{ "YAW P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_yawP, 0, 255, 1 } },
|
||||
|
||||
|
|
@ -155,11 +159,11 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
|
|||
|
||||
gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
|
||||
gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
|
||||
positionConfig_landingAltitudeM = positionConfig()->landing_altitude_m;
|
||||
positionControlConfig_landingAltitudeM = positionControlConfig()->landing_altitude_m;
|
||||
|
||||
gpsRescueConfig_throttleMin = gpsRescueConfig()->throttleMin;
|
||||
gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax;
|
||||
positionConfig_hoverThrottle = positionConfig()->hover_throttle;
|
||||
positionControlConfig_hoverThrottle = positionControlConfig()->hover_throttle;
|
||||
|
||||
gpsRescueConfig_minSats = gpsRescueConfig()->minSats;
|
||||
gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix;
|
||||
|
|
@ -183,11 +187,11 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
|
|||
|
||||
gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
|
||||
gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;
|
||||
positionConfigMutable()->landing_altitude_m = positionConfig_landingAltitudeM;
|
||||
positionControlConfigMutable()->landing_altitude_m = positionControlConfig_landingAltitudeM;
|
||||
|
||||
gpsRescueConfigMutable()->throttleMin = gpsRescueConfig_throttleMin;
|
||||
gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax;
|
||||
positionConfigMutable()->hover_throttle = positionConfig_hoverThrottle;
|
||||
positionControlConfigMutable()->hover_throttle = positionControlConfig_hoverThrottle;
|
||||
|
||||
gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats;
|
||||
gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix;
|
||||
|
|
@ -210,11 +214,11 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] =
|
|||
|
||||
{ "DESCENT DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 5, 500, 1 } },
|
||||
{ "DESCENT RATE CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 25, 500, 1 } },
|
||||
{ "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionConfig_landingAltitudeM, 1, 15, 1 } },
|
||||
{ "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_landingAltitudeM, 1, 15, 1 } },
|
||||
|
||||
{ "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMin, 1000, 2000, 1 } },
|
||||
{ "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMax, 1000, 2000, 1 } },
|
||||
{ "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &positionConfig_hoverThrottle, 1100, 1700, 1 } },
|
||||
{ "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &positionControlConfig_hoverThrottle, 1100, 1700, 1 } },
|
||||
|
||||
{ "SATS REQUIRED", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } },
|
||||
{ "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix },
|
||||
|
|
|
|||
|
|
@ -102,6 +102,7 @@
|
|||
#include "flight/pid.h"
|
||||
#include "flight/pid_init.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/servos.h"
|
||||
|
||||
#include "io/asyncfatfs/asyncfatfs.h"
|
||||
|
|
@ -762,9 +763,6 @@ void init(void)
|
|||
#ifdef USE_GPS
|
||||
if (featureIsEnabled(FEATURE_GPS)) {
|
||||
gpsInit();
|
||||
#ifdef USE_GPS_RESCUE
|
||||
gpsRescueInit();
|
||||
#endif
|
||||
#ifdef USE_GPS_LAP_TIMER
|
||||
gpsLapTimerInit();
|
||||
#endif // USE_GPS_LAP_TIMER
|
||||
|
|
@ -828,7 +826,9 @@ void init(void)
|
|||
#ifdef USE_BARO
|
||||
baroStartCalibration();
|
||||
#endif
|
||||
|
||||
positionInit();
|
||||
positionControlInit(positionControlConfig());
|
||||
|
||||
#if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL)
|
||||
vtxTableInit();
|
||||
|
|
@ -1000,10 +1000,17 @@ void init(void)
|
|||
spiInitBusDMA();
|
||||
#endif
|
||||
|
||||
// position_control must be initialised before modes that require the position_control pids
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
altHoldInit();
|
||||
#endif
|
||||
|
||||
#ifdef USE_GPS_RESCUE
|
||||
if (featureIsEnabled(FEATURE_GPS)) {
|
||||
gpsRescueInit();
|
||||
}
|
||||
#endif
|
||||
|
||||
debugInit();
|
||||
|
||||
unusedPinsInit();
|
||||
|
|
|
|||
|
|
@ -147,12 +147,18 @@
|
|||
#define PARAM_NAME_RPM_FILTER_MIN_HZ "rpm_filter_min_hz"
|
||||
#define PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ "rpm_filter_fade_range_hz"
|
||||
#define PARAM_NAME_RPM_FILTER_LPF_HZ "rpm_filter_lpf_hz"
|
||||
|
||||
#define PARAM_NAME_ALTITUDE_SOURCE "altitude_source"
|
||||
#define PARAM_NAME_ALTITUDE_PREFER_BARO "altitude_prefer_baro"
|
||||
#define PARAM_NAME_ALTITUDE_LPF "altitude_lpf"
|
||||
#define PARAM_NAME_ALTITUDE_D_LPF "altitude_d_lpf"
|
||||
#define PARAM_NAME_HOVER_THROTTLE "hover_throttle"
|
||||
#define PARAM_NAME_LANDING_ALTITUDE "landing_altitude_m"
|
||||
#define PARAM_NAME_ALTITUDE_P "altitude_P"
|
||||
#define PARAM_NAME_ALTITUDE_I "altitude_I"
|
||||
#define PARAM_NAME_ALTITUDE_D "altitude_D"
|
||||
#define PARAM_NAME_ALTITUDE_F "altitude_F"
|
||||
|
||||
#define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward"
|
||||
#define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms"
|
||||
#define PARAM_NAME_ANGLE_LIMIT "angle_limit"
|
||||
|
|
@ -207,9 +213,6 @@
|
|||
#define PARAM_NAME_GPS_RESCUE_MIN_SATS "gps_rescue_min_sats"
|
||||
#define PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX "gps_rescue_allow_arming_without_fix"
|
||||
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_P "gps_rescue_throttle_p"
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_I "gps_rescue_throttle_i"
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_D "gps_rescue_throttle_d"
|
||||
#define PARAM_NAME_GPS_RESCUE_VELOCITY_P "gps_rescue_velocity_p"
|
||||
#define PARAM_NAME_GPS_RESCUE_VELOCITY_I "gps_rescue_velocity_i"
|
||||
#define PARAM_NAME_GPS_RESCUE_VELOCITY_D "gps_rescue_velocity_d"
|
||||
|
|
@ -231,9 +234,6 @@
|
|||
#endif // USE_GPS
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#define PARAM_NAME_ALT_HOLD_P "alt_hold_p"
|
||||
#define PARAM_NAME_ALT_HOLD_I "alt_hold_i"
|
||||
#define PARAM_NAME_ALT_HOLD_D "alt_hold_d"
|
||||
#define PARAM_NAME_ALT_HOLD_THROTTLE_MIN "alt_hold_throttle_min"
|
||||
#define PARAM_NAME_ALT_HOLD_THROTTLE_MAX "alt_hold_throttle_max"
|
||||
#define PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE "alt_hold_target_adjust_rate"
|
||||
|
|
|
|||
|
|
@ -27,36 +27,23 @@
|
|||
#include "fc/rc.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "alt_hold.h"
|
||||
|
||||
typedef struct {
|
||||
float kp;
|
||||
float ki;
|
||||
float kd;
|
||||
float kf;
|
||||
float previousAltitude;
|
||||
float integral;
|
||||
} simplePid_t;
|
||||
|
||||
simplePid_t simplePid;
|
||||
static float pidIntegral = 0.0f;
|
||||
static const float taskIntervalSeconds = HZ_TO_INTERVAL(ALTHOLD_TASK_RATE_HZ); // i.e. 0.01 s
|
||||
|
||||
altHoldState_t altHoldState;
|
||||
|
||||
#define ALT_HOLD_PID_P_SCALE 0.01f
|
||||
#define ALT_HOLD_PID_I_SCALE 0.003f
|
||||
#define ALT_HOLD_PID_D_SCALE 0.01f
|
||||
|
||||
static pt2Filter_t altHoldDeltaLpf;
|
||||
static const float taskIntervalSeconds = 1.0f / ALTHOLD_TASK_RATE_HZ; // i.e. 0.01 s
|
||||
|
||||
float altitudePidCalculate(void)
|
||||
{
|
||||
// * introductory notes *
|
||||
// this is a simple PID controller with heuristic D boost and iTerm relax
|
||||
// this is a classical PID controller with heuristic D boost and iTerm relax
|
||||
// the basic parameters provide good control when initiated in stable situations
|
||||
|
||||
// tuning:
|
||||
|
|
@ -80,31 +67,32 @@ float altitudePidCalculate(void)
|
|||
// even though it may cause throttle oscillations while dropping quickly - the average D is what we need
|
||||
// - Prevent excessive iTerm growth when error is impossibly large for iTerm to resolve
|
||||
|
||||
const float altErrorCm = altHoldState.targetAltitudeCm - altHoldState.measuredAltitudeCm;
|
||||
const pidCoefficient_t *pidCoefs = getAltitudePidCoeffs();
|
||||
|
||||
const float altitudeErrorCm = altHoldState.targetAltitudeCm - getAltitudeCm();
|
||||
|
||||
// P
|
||||
const float pOut = simplePid.kp * altErrorCm;
|
||||
const float pOut = pidCoefs->Kp * altitudeErrorCm;
|
||||
|
||||
// I
|
||||
// input limit iTerm so that it doesn't grow fast with large errors
|
||||
// very important at the start if there are massive initial errors to prevent iTerm windup
|
||||
|
||||
// no iTerm change for error greater than 2m, otherwise it winds up badly
|
||||
// much less iTerm change for errors greater than 2m, otherwise it winds up badly
|
||||
const float itermNormalRange = 200.0f; // 2m
|
||||
const float itermRelax = (fabsf(altErrorCm) < itermNormalRange) ? 1.0f : 0.0f;
|
||||
simplePid.integral += altErrorCm * taskIntervalSeconds * simplePid.ki * itermRelax;
|
||||
const float itermRelax = (fabsf(altitudeErrorCm) < itermNormalRange) ? 1.0f : 0.1f;
|
||||
pidIntegral += altitudeErrorCm * pidCoefs->Ki * itermRelax * taskIntervalSeconds;
|
||||
// arbitrary limit on iTerm, same as for gps_rescue, +/-20% of full throttle range
|
||||
// ** might not be needed with input limiting **
|
||||
simplePid.integral = constrainf(simplePid.integral, -200.0f, 200.0f);
|
||||
const float iOut = simplePid.integral;
|
||||
pidIntegral = constrainf(pidIntegral, -200.0f, 200.0f);
|
||||
const float iOut = pidIntegral;
|
||||
|
||||
// D
|
||||
// boost D by 'increasing apparent velocity' when vertical velocity exceeds 5 m/s ( D of 75 on defaults)
|
||||
// the velocity trigger is arbitrary at this point
|
||||
// usually we don't see fast ascend/descend rates if the altitude hold starts under stable conditions
|
||||
// this is important primarily to arrest pre-existing fast drops or climbs at the start;
|
||||
|
||||
float vel = altHoldState.smoothedVerticalVelocity;
|
||||
float vel = getAltitudeDerivative(); // cm/s altitude derivative is always available
|
||||
const float kinkPoint = 500.0f; // velocity at which D should start to increase
|
||||
const float kinkPointAdjustment = kinkPoint * 2.0f; // Precompute constant
|
||||
const float sign = (vel > 0) ? 1.0f : -1.0f;
|
||||
|
|
@ -112,7 +100,7 @@ float altitudePidCalculate(void)
|
|||
vel = vel * 3.0f - sign * kinkPointAdjustment;
|
||||
}
|
||||
|
||||
const float dOut = simplePid.kd * vel;
|
||||
const float dOut = pidCoefs->Kd * vel;
|
||||
|
||||
// F
|
||||
// if error is used, we get a 'free kick' in derivative from changes in the target value
|
||||
|
|
@ -120,9 +108,14 @@ float altitudePidCalculate(void)
|
|||
// calculating feedforward separately avoids the filter lag.
|
||||
// Use user's D gain for the feedforward gain factor, works OK with a scaling factor of 0.01
|
||||
// A commanded drop at 100cm/s will return feedforward of the user's D value. or 15 on defaults
|
||||
const float fOut = simplePid.kf * altHoldState.targetAltitudeAdjustRate;
|
||||
const float fOut = pidCoefs->Kf * altHoldState.targetAltitudeAdjustRate;
|
||||
|
||||
// to further improve performance...
|
||||
// adding a high-pass filtered amount of FF would give a boost when altitude changes were requested
|
||||
// this would offset motor lag and kick off some initial vertical acceleration.
|
||||
// this would be exactly what an experienced pilot would do.
|
||||
|
||||
const float output = pOut + iOut + dOut + fOut;
|
||||
const float output = pOut + iOut - dOut + fOut;
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 4, lrintf(pOut));
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 5, lrintf(iOut));
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 6, lrintf(dOut));
|
||||
|
|
@ -131,43 +124,16 @@ float altitudePidCalculate(void)
|
|||
return output; // motor units, eg 100 means 10% of available throttle
|
||||
}
|
||||
|
||||
void simplePidInit(float kp, float ki, float kd, float kf)
|
||||
{
|
||||
simplePid.kp = kp;
|
||||
simplePid.ki = ki;
|
||||
simplePid.kd = kd;
|
||||
simplePid.kf = kf;
|
||||
simplePid.previousAltitude = 0.0f;
|
||||
simplePid.integral = 0.0f;
|
||||
}
|
||||
|
||||
void altHoldReset(void)
|
||||
{
|
||||
altHoldState.targetAltitudeCm = altHoldState.measuredAltitudeCm;
|
||||
simplePid.integral = 0.0f;
|
||||
pidIntegral = 0.0f;
|
||||
altHoldState.targetAltitudeCm = getAltitudeCm();
|
||||
altHoldState.targetAltitudeAdjustRate = 0.0f;
|
||||
}
|
||||
|
||||
void altHoldInit(void)
|
||||
{
|
||||
simplePidInit(
|
||||
ALT_HOLD_PID_P_SCALE * altholdConfig()->alt_hold_pid_p,
|
||||
ALT_HOLD_PID_I_SCALE * altholdConfig()->alt_hold_pid_i,
|
||||
ALT_HOLD_PID_D_SCALE * altholdConfig()->alt_hold_pid_d,
|
||||
0.01f * altholdConfig()->alt_hold_pid_d); // use D gain for feedforward with simple scaling
|
||||
// the multipliers are base scale factors
|
||||
// iTerm is relatively weak, intended to be slow moving to adjust baseline errors
|
||||
// the Hover value is important otherwise takes time for iTerm to correct
|
||||
// High P will wobble readily
|
||||
// with these scalers, the same numbers as for GPS Rescue work OK for altHold
|
||||
// the goal is to share these gain factors, if practical for all altitude controllers
|
||||
|
||||
//setup altitude D filter
|
||||
const float cutoffHz = 0.01f * positionConfig()->altitude_d_lpf; // default 1Hz, time constant about 160ms
|
||||
const float gain = pt2FilterGain(cutoffHz, taskIntervalSeconds);
|
||||
pt2FilterInit(&altHoldDeltaLpf, gain);
|
||||
|
||||
altHoldState.hover = positionConfig()->hover_throttle - PWM_RANGE_MIN;
|
||||
altHoldState.hoverThrottle = positionControlConfig()->hover_throttle - PWM_RANGE_MIN;
|
||||
altHoldState.isAltHoldActive = false;
|
||||
altHoldReset();
|
||||
}
|
||||
|
|
@ -211,8 +177,8 @@ void altHoldUpdateTargetAltitude(void)
|
|||
|
||||
const float rcThrottle = rcCommand[THROTTLE];
|
||||
|
||||
const float lowThreshold = 0.5f * (positionConfig()->hover_throttle + PWM_RANGE_MIN); // halfway between hover and MIN, e.g. 1150 if hover is 1300
|
||||
const float highThreshold = 0.5f * (positionConfig()->hover_throttle + PWM_RANGE_MAX); // halfway between hover and MAX, e.g. 1650 if hover is 1300
|
||||
const float lowThreshold = 0.5f * (positionControlConfig()->hover_throttle + PWM_RANGE_MIN); // halfway between hover and MIN, e.g. 1150 if hover is 1300
|
||||
const float highThreshold = 0.5f * (positionControlConfig()->hover_throttle + PWM_RANGE_MAX); // halfway between hover and MAX, e.g. 1650 if hover is 1300
|
||||
|
||||
float throttleAdjustmentFactor = 0.0f;
|
||||
if (rcThrottle < lowThreshold) {
|
||||
|
|
@ -230,7 +196,7 @@ void altHoldUpdateTargetAltitude(void)
|
|||
// user should be able to descend within 60s from around 150m high without disarming, on defaults
|
||||
// the deceleration may be a bit rocky if it starts very high up
|
||||
// constant (set) deceleration target in the last 2m
|
||||
throttleAdjustmentFactor = -(0.9f + constrainf(altHoldState.measuredAltitudeCm * (1.0f / 2000.f), 0.1f, 9.0f));
|
||||
throttleAdjustmentFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f));
|
||||
}
|
||||
|
||||
altHoldState.targetAltitudeAdjustRate = throttleAdjustmentFactor * altholdConfig()->alt_hold_target_adjust_rate;
|
||||
|
|
@ -251,28 +217,18 @@ void altHoldUpdate(void)
|
|||
const float tiltMultiplier = 2.0f - fmaxf(getCosTiltAngle(), 0.5f);
|
||||
// 1 = flat, 1.24 at 40 degrees, max 1.5 around 60 degrees, the default limit of Angle Mode
|
||||
// 2 - cos(x) is between 1/cos(x) and 1/sqrt(cos(x)) in this range
|
||||
const float newThrottle = PWM_RANGE_MIN + (altHoldState.hover + throttleAdjustment) * tiltMultiplier;
|
||||
const float newThrottle = PWM_RANGE_MIN + (altHoldState.hoverThrottle + throttleAdjustment) * tiltMultiplier;
|
||||
altHoldState.throttleOut = constrainf(newThrottle, altholdConfig()->alt_hold_throttle_min, altholdConfig()->alt_hold_throttle_max);
|
||||
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 0, lrintf(altHoldState.targetAltitudeCm));
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 2, lrintf(throttleAdjustment));
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 2, lrintf(newThrottle)); // normal range 1000-2000, but is beforeconstraint
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 3, lrintf(tiltMultiplier * 100));
|
||||
}
|
||||
|
||||
void updateAltHoldState(timeUs_t currentTimeUs) {
|
||||
UNUSED(currentTimeUs);
|
||||
|
||||
// things that always happen
|
||||
// calculate smoothed altitude Delta always, for effective value on 1st pass
|
||||
altHoldState.measuredAltitudeCm = getAltitude();
|
||||
float derivative = (simplePid.previousAltitude - altHoldState.measuredAltitudeCm) / taskIntervalSeconds; // cm/s
|
||||
simplePid.previousAltitude = altHoldState.measuredAltitudeCm;
|
||||
|
||||
// smooth the derivative here to always have a current value, without delay due to filter lag
|
||||
// this way we immediately have useful D on initialising the hold
|
||||
altHoldState.smoothedVerticalVelocity = pt2FilterApply(&altHoldDeltaLpf, derivative);
|
||||
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 1, lrintf(altHoldState.measuredAltitudeCm));
|
||||
UNUSED(currentTimeUs);
|
||||
|
||||
// check for enabling Alt Hold, otherwise do as little as possible while inactive
|
||||
altHoldProcessTransitions();
|
||||
|
||||
if (altHoldState.isAltHoldActive) {
|
||||
|
|
@ -281,7 +237,11 @@ void updateAltHoldState(timeUs_t currentTimeUs) {
|
|||
}
|
||||
|
||||
float altHoldGetThrottle(void) {
|
||||
return scaleRangef(altHoldState.throttleOut, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
|
||||
// see notes in gpsRescueGetThrottle() about mincheck
|
||||
float commandedThrottle = scaleRangef(altHoldState.throttleOut, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
|
||||
// with high values for mincheck, we could sclae to negative throttle values.
|
||||
commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f);
|
||||
return commandedThrottle;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -28,10 +28,8 @@ typedef struct {
|
|||
bool isAltHoldActive;
|
||||
float targetAltitudeCm;
|
||||
float targetAltitudeAdjustRate;
|
||||
float measuredAltitudeCm;
|
||||
float throttleOut;
|
||||
float hover;
|
||||
float smoothedVerticalVelocity;
|
||||
float hoverThrottle;
|
||||
} altHoldState_t;
|
||||
|
||||
void altHoldInit(void);
|
||||
|
|
|
|||
|
|
@ -44,6 +44,7 @@
|
|||
#include "flight/imu.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
|
|
@ -79,6 +80,7 @@ typedef struct {
|
|||
float maxAltitudeCm;
|
||||
float returnAltitudeCm;
|
||||
float targetAltitudeCm;
|
||||
float targetAltitudeStepCm;
|
||||
float targetVelocityCmS;
|
||||
float pitchAngleLimitDeg;
|
||||
float rollAngleLimitDeg;
|
||||
|
|
@ -95,7 +97,6 @@ typedef struct {
|
|||
} rescueIntent_s;
|
||||
|
||||
typedef struct {
|
||||
float currentAltitudeCm;
|
||||
float distanceToHomeCm;
|
||||
float distanceToHomeM;
|
||||
uint16_t groundSpeedCmS;
|
||||
|
|
@ -103,8 +104,6 @@ typedef struct {
|
|||
bool healthy;
|
||||
float errorAngle;
|
||||
float gpsDataIntervalSeconds;
|
||||
float altitudeDataIntervalSeconds;
|
||||
float gpsRescueTaskIntervalSeconds;
|
||||
float velocityToHomeCmS;
|
||||
float alitutudeStepCm;
|
||||
float maxPitchStep;
|
||||
|
|
@ -124,12 +123,12 @@ typedef struct {
|
|||
#define GPS_RESCUE_MAX_THROTTLE_ITERM 200 // max iterm value for throttle in degrees * 100
|
||||
#define GPS_RESCUE_ALLOWED_YAW_RANGE 30.0f // yaw error must be less than this to enter fly home phase, and to pitch during descend()
|
||||
|
||||
static const float taskIntervalSeconds = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ); // i.e. 0.01 s
|
||||
static float rescueThrottle;
|
||||
static float rescueYaw;
|
||||
float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
|
||||
bool magForceDisable = false;
|
||||
float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
|
||||
bool magForceDisable = false;
|
||||
static bool newGPSData = false;
|
||||
static pt2Filter_t throttleDLpf;
|
||||
static pt1Filter_t velocityDLpf;
|
||||
static pt3Filter_t velocityUpsampleLpf;
|
||||
|
||||
|
|
@ -137,13 +136,7 @@ rescueState_s rescueState;
|
|||
|
||||
void gpsRescueInit(void)
|
||||
{
|
||||
rescueState.sensor.gpsRescueTaskIntervalSeconds = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ);
|
||||
|
||||
float cutoffHz, gain;
|
||||
cutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
|
||||
gain = pt2FilterGain(cutoffHz, rescueState.sensor.gpsRescueTaskIntervalSeconds);
|
||||
pt2FilterInit(&throttleDLpf, gain);
|
||||
|
||||
cutoffHz = gpsRescueConfig()->pitchCutoffHz / 100.0f;
|
||||
rescueState.intent.velocityPidCutoff = cutoffHz;
|
||||
rescueState.intent.velocityPidCutoffModifier = 1.0f;
|
||||
|
|
@ -151,7 +144,7 @@ void gpsRescueInit(void)
|
|||
pt1FilterInit(&velocityDLpf, gain);
|
||||
|
||||
cutoffHz *= 4.0f;
|
||||
gain = pt3FilterGain(cutoffHz, rescueState.sensor.gpsRescueTaskIntervalSeconds);
|
||||
gain = pt3FilterGain(cutoffHz, taskIntervalSeconds);
|
||||
pt3FilterInit(&velocityUpsampleLpf, gain);
|
||||
}
|
||||
|
||||
|
|
@ -183,11 +176,11 @@ static void setReturnAltitude(void)
|
|||
}
|
||||
|
||||
// While armed, but not during the rescue, update the max altitude value
|
||||
rescueState.intent.maxAltitudeCm = fmaxf(rescueState.sensor.currentAltitudeCm, rescueState.intent.maxAltitudeCm);
|
||||
rescueState.intent.maxAltitudeCm = fmaxf(getAltitudeCm(), rescueState.intent.maxAltitudeCm);
|
||||
|
||||
if (newGPSData) {
|
||||
// set the target altitude to current values, so there will be no D kick on first run
|
||||
rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm;
|
||||
// set the target altitude to the current altitude.
|
||||
rescueState.intent.targetAltitudeCm = getAltitudeCm();
|
||||
|
||||
// Intended descent distance for rescues that start outside the minStartDistM distance
|
||||
// Set this to the user's intended descent distance, but not more than half the distance to home to ensure some fly home time
|
||||
|
|
@ -200,7 +193,7 @@ static void setReturnAltitude(void)
|
|||
break;
|
||||
case GPS_RESCUE_ALT_MODE_CURRENT:
|
||||
// climb above current altitude, but always return at least initial height above takeoff point, in case current altitude was negative
|
||||
rescueState.intent.returnAltitudeCm = fmaxf(initialClimbCm, rescueState.sensor.currentAltitudeCm + initialClimbCm);
|
||||
rescueState.intent.returnAltitudeCm = fmaxf(initialClimbCm, getAltitudeCm() + initialClimbCm);
|
||||
break;
|
||||
case GPS_RESCUE_ALT_MODE_MAX:
|
||||
default:
|
||||
|
|
@ -216,8 +209,6 @@ static void rescueAttainPosition(void)
|
|||
static float previousVelocityError = 0.0f;
|
||||
static float velocityI = 0.0f;
|
||||
static float throttleI = 0.0f;
|
||||
static float previousAltitudeError = 0.0f;
|
||||
static int16_t throttleAdjustment = 0;
|
||||
|
||||
switch (rescueState.phase) {
|
||||
case RESCUE_IDLE:
|
||||
|
|
@ -231,8 +222,6 @@ static void rescueAttainPosition(void)
|
|||
previousVelocityError = 0.0f;
|
||||
velocityI = 0.0f;
|
||||
throttleI = 0.0f;
|
||||
previousAltitudeError = 0.0f;
|
||||
throttleAdjustment = 0;
|
||||
rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold * 0.1f;
|
||||
rescueState.sensor.imuYawCogGain = 1.0f;
|
||||
return;
|
||||
|
|
@ -240,7 +229,7 @@ static void rescueAttainPosition(void)
|
|||
// 20s of slow descent for switch induced sanity failures to allow time to recover
|
||||
gpsRescueAngle[AI_PITCH] = 0.0f;
|
||||
gpsRescueAngle[AI_ROLL] = 0.0f;
|
||||
rescueThrottle = positionConfig()->hover_throttle - 100;
|
||||
rescueThrottle = positionControlConfig()->hover_throttle - 100;
|
||||
return;
|
||||
default:
|
||||
break;
|
||||
|
|
@ -249,47 +238,47 @@ static void rescueAttainPosition(void)
|
|||
/**
|
||||
Altitude (throttle) controller
|
||||
*/
|
||||
// currentAltitudeCm is updated at TASK_GPS_RESCUE_RATE_HZ
|
||||
const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100.0f;
|
||||
// height above target in metres (negative means too low)
|
||||
const float altitudeErrorCm = (rescueState.intent.targetAltitudeCm - getAltitudeCm());
|
||||
// height above target in cm (negative means too low)
|
||||
// at the start, the target starts at current altitude plus one step. Increases stepwise to intended value.
|
||||
|
||||
// P component
|
||||
const float throttleP = gpsRescueConfig()->throttleP * altitudeError;
|
||||
const float throttleP = getAltitudePidCoeffs()->Kp * altitudeErrorCm;
|
||||
|
||||
// I component
|
||||
throttleI += 0.1f * gpsRescueConfig()->throttleI * altitudeError * rescueState.sensor.altitudeDataIntervalSeconds;
|
||||
// reduce the iTerm gain for errors greater than 2m, otherwise it winds up too much
|
||||
const float itermNormalRange = 200.0f; // 2m
|
||||
const float itermRelax = (fabsf(altitudeErrorCm) < itermNormalRange) ? 1.0f : 0.1f;
|
||||
|
||||
throttleI += altitudeErrorCm * getAltitudePidCoeffs()->Ki * itermRelax * taskIntervalSeconds;
|
||||
throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_THROTTLE_ITERM, 1.0f * GPS_RESCUE_MAX_THROTTLE_ITERM);
|
||||
// up to 20% increase in throttle from I alone
|
||||
// up to 20% increase in throttle from I alone, need to check if this is needed, in practice.
|
||||
|
||||
// D component is error based, so includes positive boost when climbing and negative boost on descent
|
||||
float throttleD = ((altitudeError - previousAltitudeError) / rescueState.sensor.altitudeDataIntervalSeconds);
|
||||
previousAltitudeError = altitudeError;
|
||||
// increase by up to 2x when descent rate is faster
|
||||
throttleD *= rescueState.intent.throttleDMultiplier;
|
||||
// apply user's throttle D gain
|
||||
throttleD *= gpsRescueConfig()->throttleD;
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 6, lrintf(throttleD)); // throttle D before lowpass smoothing
|
||||
// smooth
|
||||
throttleD = pt2FilterApply(&throttleDLpf, throttleD);
|
||||
// D component
|
||||
const float throttleD = getAltitudeDerivative() * getAltitudePidCoeffs()->Kd * rescueState.intent.throttleDMultiplier;
|
||||
|
||||
// acceleration component not currently implemented - was needed previously due to GPS lag, maybe not needed now.
|
||||
// F component
|
||||
// add a feedforward element that is proportional to the ascend or descend rate
|
||||
const float throttleF = getAltitudePidCoeffs()->Kf * rescueState.intent.targetAltitudeStepCm * TASK_GPS_RESCUE_RATE_HZ;
|
||||
|
||||
float tiltAdjustment = 1.0f - getCosTiltAngle(); // 0 = flat, gets to 0.2 correcting on a windy day
|
||||
tiltAdjustment *= (positionConfig()->hover_throttle - 1000);
|
||||
// if hover is 1300, and adjustment .2, this gives us 0.2*300 or 60 of extra throttle, not much, but useful
|
||||
// too much and landings with lots of pitch adjustment, eg windy days, can be a problem
|
||||
const float hoverOffset = positionControlConfig()->hover_throttle - PWM_RANGE_MIN;
|
||||
|
||||
throttleAdjustment = throttleP + throttleI + throttleD + tiltAdjustment;
|
||||
const float tiltMultiplier = 2.0f - fmaxf(getCosTiltAngle(), 0.5f); // same code as alt_hold
|
||||
// 1 = flat, 1.24 at 40 degrees, max 1.5 around 60 degrees, the default limit of Angle Mode
|
||||
// 2 - cos(x) is between 1/cos(x) and 1/sqrt(cos(x)) in this range
|
||||
|
||||
rescueThrottle = positionConfig()->hover_throttle + throttleAdjustment;
|
||||
rescueThrottle = PWM_RANGE_MIN + (throttleP + throttleI - throttleD + throttleF + hoverOffset) * tiltMultiplier;
|
||||
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, lrintf(rescueThrottle)); // normal range 1000-2000 but is before constraint
|
||||
|
||||
// constrain rescue throttle
|
||||
rescueThrottle = constrainf(rescueThrottle, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax);
|
||||
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, lrintf(throttleP));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, lrintf(throttleD));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 4, lrintf(throttleI));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 5, lrintf(tiltAdjustment)); // factor that adjusts throttle based on tilt angle
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 7, lrintf(throttleAdjustment)); // pidSum; amount to add/subtract from hover throttle value
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, lrintf(tiltMultiplier * 100));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 4, lrintf(throttleP));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 5, lrintf(throttleI));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 6, lrintf(throttleD));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 7, lrintf(throttleF));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 6, lrintf(rescueThrottle)); // throttle value to use during a rescue
|
||||
|
||||
/**
|
||||
|
|
@ -415,7 +404,7 @@ static void performSanityChecks(void)
|
|||
} else if (rescueState.phase == RESCUE_INITIALIZE) {
|
||||
// Initialize these variables each time a GPS Rescue is started
|
||||
previousTimeUs = currentTimeUs;
|
||||
prevAltitudeCm = rescueState.sensor.currentAltitudeCm;
|
||||
prevAltitudeCm = getAltitudeCm();
|
||||
prevTargetAltitudeCm = rescueState.intent.targetAltitudeCm;
|
||||
previousDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
|
||||
secondsLowSats = 0;
|
||||
|
|
@ -501,10 +490,11 @@ static void performSanityChecks(void)
|
|||
|
||||
// These conditions ignore sanity mode settings, and apply in all rescues, to handle getting stuck in a climb or descend
|
||||
|
||||
const float actualAltitudeChange = rescueState.sensor.currentAltitudeCm - prevAltitudeCm;
|
||||
const float actualAltitudeChange = getAltitudeCm() - prevAltitudeCm;
|
||||
// ** possibly could use getAltitudeDerivative() for for actual altitude change, though it is smoothed
|
||||
const float targetAltitudeChange = rescueState.intent.targetAltitudeCm - prevTargetAltitudeCm;
|
||||
const float ratio = actualAltitudeChange / targetAltitudeChange;
|
||||
prevAltitudeCm = rescueState.sensor.currentAltitudeCm;
|
||||
prevAltitudeCm = getAltitudeCm();
|
||||
prevTargetAltitudeCm = rescueState.intent.targetAltitudeCm;
|
||||
|
||||
switch (rescueState.phase) {
|
||||
|
|
@ -545,17 +535,9 @@ static void performSanityChecks(void)
|
|||
static void sensorUpdate(void)
|
||||
{
|
||||
static float prevDistanceToHomeCm = 0.0f;
|
||||
const timeUs_t currentTimeUs = micros();
|
||||
|
||||
static timeUs_t previousAltitudeDataTimeUs = 0;
|
||||
const timeDelta_t altitudeDataIntervalUs = cmpTimeUs(currentTimeUs, previousAltitudeDataTimeUs);
|
||||
rescueState.sensor.altitudeDataIntervalSeconds = altitudeDataIntervalUs * 0.000001f;
|
||||
previousAltitudeDataTimeUs = currentTimeUs;
|
||||
|
||||
rescueState.sensor.currentAltitudeCm = getAltitude();
|
||||
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, lrintf(rescueState.sensor.currentAltitudeCm));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, lrintf(rescueState.sensor.currentAltitudeCm));
|
||||
const float altitudeCurrentCm = getAltitudeCm();
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, lrintf(altitudeCurrentCm));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 0, rescueState.sensor.groundSpeedCmS); // groundspeed cm/s
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 1, gpsSol.groundCourse); // degrees * 10
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 2, attitude.values.yaw); // degrees * 10
|
||||
|
|
@ -705,7 +687,7 @@ void descend(void)
|
|||
{
|
||||
if (newGPSData) {
|
||||
// consider landing area to be a circle half landing height around home, to avoid overshooting home point
|
||||
const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * positionConfig()->landing_altitude_m);
|
||||
const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * positionControlConfig()->landing_altitude_m);
|
||||
const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
|
||||
|
||||
// increase the velocity lowpass filter cutoff for more aggressive responses when descending, especially close to home
|
||||
|
|
@ -738,23 +720,24 @@ void descend(void)
|
|||
rescueState.intent.yawAttenuator = 1.0f;
|
||||
|
||||
// set the altitude step, considering the interval between altitude readings and the descent rate
|
||||
float altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
|
||||
float altitudeStepCm = taskIntervalSeconds * gpsRescueConfig()->descendRate;
|
||||
|
||||
// descend more slowly if the return home altitude was less than 20m
|
||||
const float descentRateAttenuator = constrainf (rescueState.intent.returnAltitudeCm / 2000.0f, 0.25f, 1.0f);
|
||||
altitudeStep *= descentRateAttenuator;
|
||||
altitudeStepCm *= descentRateAttenuator;
|
||||
// slowest descent rate will be 1/4 of normal when we start descending at or below 5m above take-off point.
|
||||
// otherwise a rescue initiated very low and close may not get all the way home
|
||||
|
||||
// descend faster while the quad is at higher altitudes
|
||||
const float descentRateMultiplier = constrainf(rescueState.intent.targetAltitudeCm / 5000.0f, 0.0f, 1.0f);
|
||||
altitudeStep *= 1.0f + (2.0f * descentRateMultiplier);
|
||||
altitudeStepCm *= 1.0f + (2.0f * descentRateMultiplier);
|
||||
// maximum descent rate increase is 3x default above 50m, 2x above 25m, 1.2x at 5m, default by ground level
|
||||
|
||||
// also increase throttle D up to 2x in the descent phase when altitude descent rate is faster, for better control
|
||||
rescueState.intent.throttleDMultiplier = 1.0f + descentRateMultiplier;
|
||||
|
||||
rescueState.intent.targetAltitudeCm -= altitudeStep;
|
||||
rescueState.intent.targetAltitudeStepCm = -altitudeStepCm;
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.targetAltitudeStepCm;
|
||||
}
|
||||
|
||||
void initialiseRescueValues (void)
|
||||
|
|
@ -768,6 +751,7 @@ void initialiseRescueValues (void)
|
|||
rescueState.intent.pitchAngleLimitDeg = 0.0f; // force pitch adjustment to zero - level mode will level out
|
||||
rescueState.intent.velocityItermAttenuator = 1.0f; // allow iTerm to accumulate normally unless constrained by IMU error or descent phase
|
||||
rescueState.intent.velocityItermRelax = 0.0f; // but don't accumulate any at the start, not until fly home
|
||||
rescueState.intent.targetAltitudeStepCm = 0.0f;
|
||||
}
|
||||
|
||||
void gpsRescueUpdate(void)
|
||||
|
|
@ -805,21 +789,21 @@ void gpsRescueUpdate(void)
|
|||
rescueState.failure = RESCUE_NO_HOME_POINT;
|
||||
// will result in a disarm via the sanity check system, or float around if switch induced
|
||||
} else {
|
||||
if (rescueState.sensor.distanceToHomeM < 5.0f && isAltitudeLow()) {
|
||||
if (rescueState.sensor.distanceToHomeM < 5.0f && isBelowLandingAltitude()) {
|
||||
// attempted initiation within 5m of home, and 'on the ground' -> instant disarm, for safety reasons
|
||||
rescueState.phase = RESCUE_ABORT;
|
||||
} else {
|
||||
// attempted initiation within minimum rescue distance requires us to fly out to at least that distance
|
||||
if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minStartDistM) {
|
||||
// climb above current height by buffer height, to at least 10m altitude
|
||||
rescueState.intent.returnAltitudeCm = fmaxf(1000.0f, rescueState.sensor.currentAltitudeCm + (gpsRescueConfig()->initialClimbM * 100.0f));
|
||||
rescueState.intent.returnAltitudeCm = fmaxf(1000.0f, getAltitudeCm() + (gpsRescueConfig()->initialClimbM * 100.0f));
|
||||
// note that the pitch controller will pitch forward to fly out to minStartDistM
|
||||
// set the descent distance to half the minimum rescue distance. which we should have moved out to in the climb phase
|
||||
rescueState.intent.descentDistanceM = 0.5f * gpsRescueConfig()->minStartDistM;
|
||||
}
|
||||
// otherwise behave as for a normal rescue
|
||||
initialiseRescueValues ();
|
||||
returnAltitudeLow = (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm);
|
||||
returnAltitudeLow = getAltitudeCm() < rescueState.intent.returnAltitudeCm;
|
||||
rescueState.phase = RESCUE_ATTAIN_ALT;
|
||||
}
|
||||
}
|
||||
|
|
@ -829,13 +813,15 @@ void gpsRescueUpdate(void)
|
|||
// gradually increment the target altitude until the craft reaches target altitude
|
||||
// note that this can mean the target altitude may increase above returnAltitude if the craft lags target
|
||||
// sanity check will abort if altitude gain is blocked for a cumulative period
|
||||
if (returnAltitudeLow == (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm)) {
|
||||
if (returnAltitudeLow == (getAltitudeCm() < rescueState.intent.returnAltitudeCm)) {
|
||||
// we started low, and still are low; also true if we started high, and still are too high
|
||||
const float altitudeStep = (returnAltitudeLow ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * rescueState.sensor.gpsRescueTaskIntervalSeconds;
|
||||
rescueState.intent.targetAltitudeCm += altitudeStep;
|
||||
rescueState.intent.targetAltitudeStepCm = (returnAltitudeLow ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * taskIntervalSeconds;
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.targetAltitudeStepCm;
|
||||
|
||||
} else {
|
||||
// target altitude achieved - move on to ROTATE phase, returning at target altitude
|
||||
rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
|
||||
rescueState.intent.targetAltitudeStepCm = 0.0f;
|
||||
// if initiated too close, do not rotate or do anything else until sufficiently far away that a 'normal' rescue can happen
|
||||
if (rescueState.sensor.distanceToHomeM > gpsRescueConfig()->minStartDistM) {
|
||||
rescueState.phase = RESCUE_ROTATE;
|
||||
|
|
@ -848,7 +834,7 @@ void gpsRescueUpdate(void)
|
|||
|
||||
case RESCUE_ROTATE:
|
||||
if (rescueState.intent.yawAttenuator < 1.0f) { // acquire yaw authority over one second
|
||||
rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
|
||||
rescueState.intent.yawAttenuator += taskIntervalSeconds;
|
||||
}
|
||||
if (rescueState.sensor.absErrorAngle < GPS_RESCUE_ALLOWED_YAW_RANGE) {
|
||||
// enter fly home phase, and enable pitch, when the yaw angle error is small enough
|
||||
|
|
@ -862,12 +848,12 @@ void gpsRescueUpdate(void)
|
|||
|
||||
case RESCUE_FLY_HOME:
|
||||
if (rescueState.intent.yawAttenuator < 1.0f) { // be sure to accumulate full yaw authority
|
||||
rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
|
||||
rescueState.intent.yawAttenuator += taskIntervalSeconds;
|
||||
}
|
||||
// velocity PIDs are now active
|
||||
// update target velocity gradually, aiming for rescueGroundspeed with a time constant of 1.0s
|
||||
const float targetVelocityError = gpsRescueConfig()->groundSpeedCmS - rescueState.intent.targetVelocityCmS;
|
||||
const float velocityTargetStep = rescueState.sensor.gpsRescueTaskIntervalSeconds * targetVelocityError;
|
||||
const float velocityTargetStep = taskIntervalSeconds * targetVelocityError;
|
||||
// velocityTargetStep is positive when starting low, negative when starting high
|
||||
const bool targetVelocityIsLow = rescueState.intent.targetVelocityCmS < gpsRescueConfig()->groundSpeedCmS;
|
||||
if (initialVelocityLow == targetVelocityIsLow) {
|
||||
|
|
@ -876,7 +862,7 @@ void gpsRescueUpdate(void)
|
|||
}
|
||||
|
||||
// slowly introduce velocity iTerm accumulation at start, goes 0 ->1 with time constant 2.0s
|
||||
rescueState.intent.velocityItermRelax += 0.5f * rescueState.sensor.gpsRescueTaskIntervalSeconds * (1.0f - rescueState.intent.velocityItermRelax);
|
||||
rescueState.intent.velocityItermRelax += 0.5f * taskIntervalSeconds * (1.0f - rescueState.intent.velocityItermRelax);
|
||||
// there is always a lot of lag at the start, this gradual start avoids excess iTerm accumulation
|
||||
|
||||
rescueState.intent.velocityPidCutoffModifier = 2.0f - rescueState.intent.velocityItermRelax;
|
||||
|
|
@ -896,7 +882,7 @@ void gpsRescueUpdate(void)
|
|||
|
||||
case RESCUE_DESCENT:
|
||||
// attenuate velocity and altitude targets while updating the heading to home
|
||||
if (isAltitudeLow()) {
|
||||
if (isBelowLandingAltitude()) {
|
||||
// enter landing mode once below landing altitude
|
||||
rescueState.phase = RESCUE_LANDING;
|
||||
rescueState.intent.secondsFailing = 0; // reset sanity timer for landing
|
||||
|
|
@ -932,7 +918,7 @@ void gpsRescueUpdate(void)
|
|||
}
|
||||
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 3, lrintf(rescueState.intent.targetAltitudeCm));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, lrintf(rescueState.intent.targetAltitudeCm));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, lrintf(rescueState.intent.targetAltitudeCm));
|
||||
DEBUG_SET(DEBUG_RTH, 0, lrintf(rescueState.intent.maxAltitudeCm / 10.0f));
|
||||
|
||||
performSanityChecks();
|
||||
|
|
@ -953,12 +939,15 @@ float gpsRescueGetImuYawCogGain(void)
|
|||
|
||||
float gpsRescueGetThrottle(void)
|
||||
{
|
||||
// Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer.
|
||||
// We need to compensate for min_check since the throttle value set by gps rescue
|
||||
// is based on the raw rcCommand value commanded by the pilot.
|
||||
// Calculate the commanded throttle for use in the mixer. Output must be within range 0.0 - 1.0.
|
||||
// minCheck can be less than, or greater than, PWM_RANGE_MIN, but is usually at default of 1050
|
||||
// it is the value at which the user expects the motors to start spinning (leaving a deadband from 1000 to 1050)
|
||||
// rescue throttle min can't be less than gps_rescue_throttle_min (1100) or greater than max (1750)
|
||||
// we scale throttle from mincheck to PWM_RANGE_MAX when mincheck is greater than PWM_RANGE_MIN, otherwise from PWM_RANGE_MIN to PWM_RANGE_MAX
|
||||
float commandedThrottle = scaleRangef(rescueThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
|
||||
// if mincheck is set below PWRM_RANGE_MIN, the gps rescue throttle may seem greater than expected
|
||||
// with high values for mincheck, we could sclae to negative throttle values.
|
||||
commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f);
|
||||
|
||||
return commandedThrottle;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -50,7 +50,6 @@ extern float gpsRescueAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGR
|
|||
void gpsRescueInit(void);
|
||||
void gpsRescueUpdate(void);
|
||||
void gpsRescueNewGpsData(void);
|
||||
|
||||
float gpsRescueGetYawRate(void);
|
||||
float gpsRescueGetThrottle(void);
|
||||
bool gpsRescueIsConfigured(void);
|
||||
|
|
|
|||
|
|
@ -50,7 +50,7 @@
|
|||
#include "flight/gps_rescue.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/rpm_filter.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
|
@ -815,7 +815,7 @@ static FAST_CODE_NOINLINE void disarmOnImpact(void)
|
|||
// for more reliable disarm with gentle controlled landings
|
||||
float lowAltitudeSensitivity = 1.0f;
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
lowAltitudeSensitivity = (FLIGHT_MODE(ALT_HOLD_MODE) && isAltitudeLow()) ? 1.5f : 1.0f;
|
||||
lowAltitudeSensitivity = (FLIGHT_MODE(ALT_HOLD_MODE) && isBelowLandingAltitude()) ? 1.5f : 1.0f;
|
||||
#endif
|
||||
// and disarm if accelerometer jerk exceeds threshold...
|
||||
if ((fabsf(acc.accDelta) * lowAltitudeSensitivity) > pidRuntime.landingDisarmThreshold) {
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@
|
|||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/pid.h"
|
||||
|
||||
|
|
@ -48,14 +49,10 @@
|
|||
#include "pg/pg_ids.h"
|
||||
|
||||
static float displayAltitudeCm = 0.0f;
|
||||
static float zeroedAltitudeCm = 0.0f;
|
||||
static bool altitudeAvailable = false;
|
||||
static bool altitudeIsLow = false;
|
||||
|
||||
#if defined(USE_BARO) || defined(USE_GPS)
|
||||
|
||||
static float zeroedAltitudeCm = 0.0f;
|
||||
static float zeroedAltitudeDerivative = 0.0f;
|
||||
#endif
|
||||
|
||||
static pt2Filter_t altitudeLpf;
|
||||
static pt2Filter_t altitudeDerivativeLpf;
|
||||
|
|
@ -83,15 +80,13 @@ typedef enum {
|
|||
GPS_ONLY
|
||||
} altitudeSource_e;
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 5);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 6);
|
||||
|
||||
PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
|
||||
.altitude_source = DEFAULT,
|
||||
.altitude_prefer_baro = 100, // percentage 'trust' of baro data
|
||||
.altitude_lpf = 300,
|
||||
.altitude_d_lpf = 100,
|
||||
.hover_throttle = 1275,
|
||||
.landing_altitude_m = 4,
|
||||
);
|
||||
|
||||
#if defined(USE_BARO) || defined(USE_GPS)
|
||||
|
|
@ -202,9 +197,6 @@ void calculateEstimatedAltitude(void)
|
|||
zeroedAltitudeDerivative = (zeroedAltitudeCm - previousZeroedAltitudeCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
|
||||
previousZeroedAltitudeCm = zeroedAltitudeCm;
|
||||
|
||||
// assess if altitude is low here, only when we get new data, rather than in pid loop etc
|
||||
altitudeIsLow = zeroedAltitudeCm < 100.0f * positionConfig()->landing_altitude_m;
|
||||
|
||||
zeroedAltitudeDerivative = pt2FilterApply(&altitudeDerivativeLpf, zeroedAltitudeDerivative);
|
||||
|
||||
#ifdef USE_VARIO
|
||||
|
|
@ -219,11 +211,24 @@ void calculateEstimatedAltitude(void)
|
|||
DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
|
||||
#endif
|
||||
DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f));
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 1, lrintf(zeroedAltitudeCm));
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, lrintf(zeroedAltitudeCm));
|
||||
|
||||
altitudeAvailable = haveGpsAlt || haveBaroAlt;
|
||||
}
|
||||
|
||||
#endif //defined(USE_BARO) || defined(USE_GPS)
|
||||
|
||||
float getAltitudeCm(void)
|
||||
{
|
||||
return zeroedAltitudeCm;
|
||||
}
|
||||
|
||||
float getAltitudeDerivative(void)
|
||||
{
|
||||
return zeroedAltitudeDerivative; // cm/s
|
||||
}
|
||||
|
||||
bool isAltitudeAvailable(void) {
|
||||
return altitudeAvailable;
|
||||
}
|
||||
|
|
@ -233,16 +238,6 @@ int32_t getEstimatedAltitudeCm(void)
|
|||
return lrintf(displayAltitudeCm);
|
||||
}
|
||||
|
||||
float getAltitude(void)
|
||||
{
|
||||
return zeroedAltitudeCm;
|
||||
}
|
||||
|
||||
bool isAltitudeLow(void)
|
||||
{
|
||||
return altitudeIsLow;
|
||||
}
|
||||
|
||||
#ifdef USE_GPS
|
||||
float getAltitudeAsl(void)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -29,17 +29,16 @@ typedef struct positionConfig_s {
|
|||
uint8_t altitude_prefer_baro;
|
||||
uint16_t altitude_lpf; // lowpass cutoff (value / 100) Hz for altitude smoothing
|
||||
uint16_t altitude_d_lpf; // lowpass for (value / 100) Hz for altitude derivative smoothing
|
||||
uint16_t hover_throttle; // value used at the start of a rescue or position hold
|
||||
uint8_t landing_altitude_m; // altitude below which landing behaviours can change, metres
|
||||
} positionConfig_t;
|
||||
|
||||
PG_DECLARE(positionConfig_t, positionConfig);
|
||||
|
||||
float getAltitudeCm(void);
|
||||
float getAltitudeDerivative(void);
|
||||
void calculateEstimatedAltitude(void);
|
||||
void positionInit(void);
|
||||
int32_t getEstimatedAltitudeCm(void);
|
||||
float getAltitude(void);
|
||||
bool isAltitudeLow(void);
|
||||
float getAltitudeAsl(void);
|
||||
int16_t getEstimatedVario(void);
|
||||
bool isAltitudeAvailable(void);
|
||||
|
||||
|
|
|
|||
50
src/main/flight/position_control.c
Normal file
50
src/main/flight/position_control.c
Normal file
|
|
@ -0,0 +1,50 @@
|
|||
/*
|
||||
* This file is part of Betaflight.
|
||||
*
|
||||
* Betaflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Betaflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "position.h"
|
||||
#include "position_control.h"
|
||||
|
||||
#define ALTITUDE_P_SCALE 0.01f
|
||||
#define ALTITUDE_I_SCALE 0.003f
|
||||
#define ALTITUDE_D_SCALE 0.01f
|
||||
#define ALTITUDE_F_SCALE 0.01f
|
||||
|
||||
static pidCoefficient_t altitudePidCoeffs;
|
||||
|
||||
void positionControlInit(const positionControlConfig_t *config)
|
||||
{
|
||||
altitudePidCoeffs.Kp = config->altitude_P * ALTITUDE_P_SCALE;
|
||||
altitudePidCoeffs.Ki = config->altitude_I * ALTITUDE_I_SCALE;
|
||||
altitudePidCoeffs.Kd = config->altitude_D * ALTITUDE_D_SCALE;
|
||||
altitudePidCoeffs.Kf = config->altitude_F * ALTITUDE_F_SCALE;
|
||||
}
|
||||
|
||||
bool isBelowLandingAltitude(void)
|
||||
{
|
||||
return getAltitudeCm() < 100.0f * positionControlConfig()->landing_altitude_m;
|
||||
}
|
||||
|
||||
const pidCoefficient_t *getAltitudePidCoeffs(void)
|
||||
{
|
||||
return &altitudePidCoeffs;
|
||||
}
|
||||
27
src/main/flight/position_control.h
Normal file
27
src/main/flight/position_control.h
Normal file
|
|
@ -0,0 +1,27 @@
|
|||
/*
|
||||
* This file is part of Betaflight.
|
||||
*
|
||||
* Betaflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Betaflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "pg/position_control.h"
|
||||
#include "flight/pid.h"
|
||||
|
||||
void positionControlInit(const positionControlConfig_t *config);
|
||||
|
||||
bool isBelowLandingAltitude(void);
|
||||
|
||||
const pidCoefficient_t *getAltitudePidCoeffs(void);
|
||||
|
|
@ -89,6 +89,7 @@
|
|||
#include "flight/pid.h"
|
||||
#include "flight/pid_init.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/rpm_filter.h"
|
||||
#include "flight/servos.h"
|
||||
|
||||
|
|
@ -1545,7 +1546,7 @@ case MSP_NAME:
|
|||
sbufWriteU16(dst, gpsRescueConfig()->groundSpeedCmS);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->throttleMin);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->throttleMax);
|
||||
sbufWriteU16(dst, positionConfig()->hover_throttle);
|
||||
sbufWriteU16(dst, positionControlConfig()->hover_throttle);
|
||||
sbufWriteU8(dst, gpsRescueConfig()->sanityChecks);
|
||||
sbufWriteU8(dst, gpsRescueConfig()->minSats);
|
||||
|
||||
|
|
@ -1561,9 +1562,10 @@ case MSP_NAME:
|
|||
break;
|
||||
|
||||
case MSP_GPS_RESCUE_PIDS:
|
||||
sbufWriteU16(dst, gpsRescueConfig()->throttleP);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->throttleI);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->throttleD);
|
||||
sbufWriteU16(dst, positionControlConfig()->altitude_P);
|
||||
sbufWriteU16(dst, positionControlConfig()->altitude_I);
|
||||
sbufWriteU16(dst, positionControlConfig()->altitude_D);
|
||||
// altitude_F not implemented yet
|
||||
sbufWriteU16(dst, gpsRescueConfig()->velP);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->velI);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->velD);
|
||||
|
|
@ -2879,7 +2881,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
gpsRescueConfigMutable()->groundSpeedCmS = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->throttleMin = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->throttleMax = sbufReadU16(src);
|
||||
positionConfigMutable()->hover_throttle = sbufReadU16(src);
|
||||
positionControlConfigMutable()->hover_throttle = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src);
|
||||
gpsRescueConfigMutable()->minSats = sbufReadU8(src);
|
||||
if (sbufBytesRemaining(src) >= 6) {
|
||||
|
|
@ -2900,9 +2902,10 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
break;
|
||||
|
||||
case MSP_SET_GPS_RESCUE_PIDS:
|
||||
gpsRescueConfigMutable()->throttleP = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->throttleI = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->throttleD = sbufReadU16(src);
|
||||
positionControlConfigMutable()->altitude_P = sbufReadU16(src);
|
||||
positionControlConfigMutable()->altitude_I = sbufReadU16(src);
|
||||
positionControlConfigMutable()->altitude_D = sbufReadU16(src);
|
||||
// altitude_F not included in msp yet
|
||||
gpsRescueConfigMutable()->velP = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->velI = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->velD = sbufReadU16(src);
|
||||
|
|
|
|||
|
|
@ -29,12 +29,9 @@
|
|||
|
||||
#include "alt_hold.h"
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 3);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 4);
|
||||
|
||||
PG_RESET_TEMPLATE(altholdConfig_t, altholdConfig,
|
||||
.alt_hold_pid_p = 15,
|
||||
.alt_hold_pid_i = 15,
|
||||
.alt_hold_pid_d = 15,
|
||||
.alt_hold_target_adjust_rate = 100, // max rate of change of altitude target using sticks in cm/s
|
||||
.alt_hold_throttle_min = 1100,
|
||||
.alt_hold_throttle_max = 1700,
|
||||
|
|
|
|||
|
|
@ -25,9 +25,6 @@
|
|||
#include "pg/pg.h"
|
||||
|
||||
typedef struct altholdConfig_s {
|
||||
uint8_t alt_hold_pid_p;
|
||||
uint8_t alt_hold_pid_i;
|
||||
uint8_t alt_hold_pid_d;
|
||||
uint8_t alt_hold_target_adjust_rate;
|
||||
uint16_t alt_hold_throttle_min;
|
||||
uint16_t alt_hold_throttle_max;
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@
|
|||
|
||||
#include "gps_rescue.h"
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 6);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 7);
|
||||
|
||||
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
||||
|
||||
|
|
@ -55,9 +55,6 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
|||
.sanityChecks = RESCUE_SANITY_FS_ONLY,
|
||||
.minSats = 8,
|
||||
|
||||
.throttleP = 15,
|
||||
.throttleI = 15,
|
||||
.throttleD = 20,
|
||||
.velP = 8,
|
||||
.velI = 40,
|
||||
.velD = 12,
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@ typedef struct gpsRescue_s {
|
|||
uint16_t returnAltitudeM; // meters
|
||||
uint16_t descentDistanceM; // meters
|
||||
uint16_t groundSpeedCmS; // centimeters per second
|
||||
uint8_t throttleP, throttleI, throttleD;
|
||||
uint8_t yawP;
|
||||
uint16_t throttleMin;
|
||||
uint16_t throttleMax;
|
||||
|
|
|
|||
|
|
@ -83,6 +83,7 @@
|
|||
#define PG_VTX_IO_CONFIG 57
|
||||
#define PG_GPS_LAP_TIMER 58
|
||||
#define PG_ALTHOLD_CONFIG 59
|
||||
#define PG_POSITION_CONTROL 60
|
||||
|
||||
// Driver configuration
|
||||
#define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight
|
||||
|
|
|
|||
39
src/main/pg/position_control.c
Normal file
39
src/main/pg/position_control.c
Normal file
|
|
@ -0,0 +1,39 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "flight/position_control.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
#include "position_control.h"
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
|
||||
|
||||
PG_RESET_TEMPLATE(positionControlConfig_t, positionControlConfig,
|
||||
.hover_throttle = 1275,
|
||||
.landing_altitude_m = 4,
|
||||
.altitude_P = 15,
|
||||
.altitude_I = 15,
|
||||
.altitude_D = 15,
|
||||
.altitude_F = 15,
|
||||
);
|
||||
37
src/main/pg/position_control.h
Normal file
37
src/main/pg/position_control.h
Normal file
|
|
@ -0,0 +1,37 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "pg/pg.h"
|
||||
|
||||
typedef struct positionControlConfig_s {
|
||||
uint16_t hover_throttle; // value used at the start of a rescue or position hold
|
||||
uint8_t landing_altitude_m; // altitude below which landing behaviours can change, metres
|
||||
uint8_t altitude_P;
|
||||
uint8_t altitude_I;
|
||||
uint8_t altitude_D;
|
||||
uint8_t altitude_F;
|
||||
} positionControlConfig_t;
|
||||
|
||||
PG_DECLARE(positionControlConfig_t, positionControlConfig);
|
||||
|
||||
|
|
@ -34,6 +34,8 @@ extern "C" {
|
|||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/pid.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
|
|
@ -41,6 +43,7 @@ extern "C" {
|
|||
|
||||
PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
|
||||
PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
|
||||
PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
|
||||
PG_REGISTER(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 0);
|
||||
|
||||
extern altHoldState_t altHoldState;
|
||||
|
|
@ -97,6 +100,13 @@ TEST(AltholdUnittest, altHoldTransitionsTestUnfinishedExitEnter)
|
|||
extern "C" {
|
||||
acc_t acc;
|
||||
|
||||
pidCoefficient_t testAltitudePidCoeffs = {15.0f, 15.0f, 15.1f, 15.0f};
|
||||
const pidCoefficient_t *getAltitudePidCoeffs(void) {
|
||||
return &testAltitudePidCoeffs;
|
||||
}
|
||||
float getAltitudeCm(void) { return 0.0f;}
|
||||
float getAltitudeDerivative(void) { return 0.0f;}
|
||||
|
||||
void pt2FilterInit(pt2Filter_t *altHoldDeltaLpf, float) {
|
||||
UNUSED(altHoldDeltaLpf);
|
||||
}
|
||||
|
|
@ -109,8 +119,6 @@ extern "C" {
|
|||
}
|
||||
|
||||
bool isAltitudeAvailable(void) { return true; }
|
||||
float getAltitude(void) { return 0.0f; }
|
||||
bool isAltitudeLow(void) { return true; }
|
||||
float getCosTiltAngle(void) { return 0.0f; }
|
||||
float rcCommand[4];
|
||||
|
||||
|
|
|
|||
|
|
@ -42,6 +42,7 @@ extern "C" {
|
|||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/servos.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
|
|
@ -77,6 +78,7 @@ extern "C" {
|
|||
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
|
||||
PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
|
||||
PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
|
||||
PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
|
||||
|
||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
uint16_t averageSystemLoadPercent = 0;
|
||||
|
|
@ -1134,17 +1136,17 @@ extern "C" {
|
|||
bool isMotorProtocolEnabled(void) { return true; }
|
||||
void pinioBoxTaskControl(void) {}
|
||||
void schedulerSetNextStateTime(timeDelta_t) {}
|
||||
float getAltitude(void) { return 3000.0f; }
|
||||
|
||||
pidCoefficient_t testAltitudePidCoeffs = {15.0f, 15.0f, 15.1f, 15.0f};
|
||||
const pidCoefficient_t *getAltitudePidCoeffs(void) {
|
||||
return &testAltitudePidCoeffs;
|
||||
}
|
||||
float getAltitudeCm(void) {return 0.0f;}
|
||||
float getAltitudeDerivative(void) {return 0.0f;}
|
||||
|
||||
float pt1FilterGain(float, float) { return 0.5f; }
|
||||
float pt2FilterGain(float, float) { return 0.1f; }
|
||||
float pt3FilterGain(float, float) { return 0.1f; }
|
||||
void pt2FilterInit(pt2Filter_t *throttleDLpf, float) {
|
||||
UNUSED(throttleDLpf);
|
||||
}
|
||||
float pt2FilterApply(pt2Filter_t *throttleDLpf, float) {
|
||||
UNUSED(throttleDLpf);
|
||||
return 0.0f;
|
||||
}
|
||||
void pt1FilterInit(pt1Filter_t *velocityDLpf, float) {
|
||||
UNUSED(velocityDLpf);
|
||||
}
|
||||
|
|
@ -1161,6 +1163,6 @@ extern "C" {
|
|||
}
|
||||
void getRcDeflectionAbs(void) {}
|
||||
uint32_t getCpuPercentageLate(void) { return 0; }
|
||||
bool crashFlipSuccessful(void) {return false; }
|
||||
bool isAltitudeLow(void) {return false; }
|
||||
bool crashFlipSuccessful(void) { return false; }
|
||||
bool isBelowLandingAltitude(void) { return false; };
|
||||
}
|
||||
|
|
|
|||
|
|
@ -46,8 +46,8 @@ extern "C" {
|
|||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
|
||||
|
||||
#include "io/gps.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
|
@ -74,6 +74,7 @@ extern "C" {
|
|||
PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
|
||||
PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0);
|
||||
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
|
||||
PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
|
||||
|
||||
PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
|
||||
.enabledFeatures = 0
|
||||
|
|
@ -438,7 +439,6 @@ extern "C" {
|
|||
float getBaroAltitude(void) { return 3000.0f; }
|
||||
float gpsRescueGetImuYawCogGain(void) { return 1.0f; }
|
||||
float getRcDeflectionAbs(int) { return 0.0f; }
|
||||
|
||||
void pt2FilterInit(pt2Filter_t *baroDerivativeLpf, float) {
|
||||
UNUSED(baroDerivativeLpf);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -101,7 +101,7 @@ extern "C" {
|
|||
float mixerGetRcThrottle() { return fabsf(simulatedMixerGetRcThrottle); }
|
||||
|
||||
|
||||
bool isAltitudeLow(void) { return false; }
|
||||
bool isBelowLandingAltitude(void) { return false; }
|
||||
|
||||
void systemBeep(bool) { }
|
||||
bool gyroOverflowDetected(void) { return false; }
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue