mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 11:29:58 +03:00
Update external defines for ALT / POS HOLD (#14069)
* Update external defines for ALT / POS HOLD * Update unit test define
This commit is contained in:
parent
b5ab83b3bc
commit
2426f43502
21 changed files with 39 additions and 39 deletions
|
@ -1815,12 +1815,12 @@ static bool blackboxWriteSysinfo(void)
|
|||
#endif // USE_GPS_RESCUE
|
||||
#endif // USE_GPS
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, "%d", altHoldConfig()->alt_hold_adjust_rate);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_DEADBAND, "%d", altHoldConfig()->alt_hold_deadband);
|
||||
#endif
|
||||
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_WITHOUT_MAG, "%d", posHoldConfig()->pos_hold_without_mag);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_DEADBAND, "%d", posHoldConfig()->pos_hold_deadband);
|
||||
#endif
|
||||
|
|
|
@ -1108,12 +1108,12 @@ const clivalue_t valueTable[] = {
|
|||
{ PARAM_NAME_YAW_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) },
|
||||
{ "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
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
{ PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_adjust_rate) },
|
||||
{ PARAM_NAME_ALT_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 70 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_deadband) },
|
||||
#endif
|
||||
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
{ PARAM_NAME_POS_HOLD_WITHOUT_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_without_mag) },
|
||||
{ PARAM_NAME_POS_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_deadband) },
|
||||
#endif
|
||||
|
|
|
@ -1016,10 +1016,10 @@ void processRxModes(timeUs_t currentTimeUs)
|
|||
bool canUseHorizonMode = true;
|
||||
if ((IS_RC_MODE_ACTIVE(BOXANGLE)
|
||||
|| failsafeIsActive()
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
|| FLIGHT_MODE(ALT_HOLD_MODE)
|
||||
#endif
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
|| FLIGHT_MODE(POS_HOLD_MODE)
|
||||
#endif
|
||||
) && (sensors(SENSOR_ACC))) {
|
||||
|
@ -1033,7 +1033,7 @@ void processRxModes(timeUs_t currentTimeUs)
|
|||
DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
|
||||
}
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
// only if armed; can coexist with position hold
|
||||
if (ARMING_FLAG(ARMED)
|
||||
// and not in GPS_RESCUE_MODE, to give it priority over Altitude Hold
|
||||
|
@ -1054,7 +1054,7 @@ void processRxModes(timeUs_t currentTimeUs)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
// only if armed; can coexist with altitude hold
|
||||
if (ARMING_FLAG(ARMED)
|
||||
// and not in GPS_RESCUE_MODE, to give it priority over Position Hold
|
||||
|
|
|
@ -1009,11 +1009,11 @@ void init(void)
|
|||
#endif
|
||||
|
||||
// autopilot must be initialised before modes that require the autopilot pids
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
altHoldInit();
|
||||
#endif
|
||||
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
posHoldInit();
|
||||
#endif
|
||||
|
||||
|
|
|
@ -247,12 +247,12 @@
|
|||
|
||||
#endif // USE_GPS
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
#define PARAM_NAME_ALT_HOLD_DEADBAND "alt_hold_deadband"
|
||||
#define PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE "alt_hold_throttle_response"
|
||||
#endif
|
||||
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
#define PARAM_NAME_POS_HOLD_WITHOUT_MAG "pos_hold_without_mag"
|
||||
#define PARAM_NAME_POS_HOLD_DEADBAND "pos_hold_deadband"
|
||||
#endif
|
||||
|
|
|
@ -378,11 +378,11 @@ task_attribute_t task_attributes[TASK_COUNT] = {
|
|||
[TASK_GPS_RESCUE] = DEFINE_TASK("GPS_RESCUE", NULL, NULL, taskGpsRescue, TASK_PERIOD_HZ(TASK_GPS_RESCUE_RATE_HZ), TASK_PRIORITY_MEDIUM),
|
||||
#endif
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
[TASK_ALTHOLD] = DEFINE_TASK("ALTHOLD", NULL, NULL, updateAltHold, TASK_PERIOD_HZ(ALTHOLD_TASK_RATE_HZ), TASK_PRIORITY_LOW),
|
||||
#endif
|
||||
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
[TASK_POSHOLD] = DEFINE_TASK("POSHOLD", NULL, NULL, updatePosHold, TASK_PERIOD_HZ(POSHOLD_TASK_RATE_HZ), TASK_PRIORITY_LOW),
|
||||
#endif
|
||||
|
||||
|
@ -544,11 +544,11 @@ void tasksInit(void)
|
|||
setTaskEnabled(TASK_GPS_RESCUE, featureIsEnabled(FEATURE_GPS));
|
||||
#endif
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
setTaskEnabled(TASK_ALTHOLD, sensors(SENSOR_BARO) || featureIsEnabled(FEATURE_GPS));
|
||||
#endif
|
||||
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
setTaskEnabled(TASK_POSHOLD, featureIsEnabled(FEATURE_GPS));
|
||||
#endif
|
||||
|
||||
|
|
|
@ -18,7 +18,7 @@
|
|||
#include "platform.h"
|
||||
#include "math.h"
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
|
||||
#include "build/debug.h"
|
||||
#include "common/maths.h"
|
||||
|
|
|
@ -19,7 +19,7 @@
|
|||
|
||||
#include "pg/alt_hold.h"
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
#include "common/time.h"
|
||||
|
||||
#define ALTHOLD_TASK_RATE_HZ 100 // hz
|
||||
|
|
|
@ -798,7 +798,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
// Throttle value to be used during altitude hold mode (and failsafe landing mode)
|
||||
if (FLIGHT_MODE(ALT_HOLD_MODE)) {
|
||||
throttle = getAutopilotThrottle();
|
||||
|
|
|
@ -570,7 +570,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
|
|||
#ifdef USE_GPS_RESCUE
|
||||
angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
|
||||
#endif
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
if (FLIGHT_MODE(POS_HOLD_MODE)) {
|
||||
angleFeedforward = 0.0f; // otherwise the lag of the PT3 carries recent stick inputs into the hold
|
||||
if (isAutopilotInControl()) {
|
||||
|
@ -942,7 +942,7 @@ static FAST_CODE_NOINLINE void disarmOnImpact(void)
|
|||
if (wasThrottleRaised()
|
||||
// and, either sticks are centred and throttle zeroed,
|
||||
&& ((getMaxRcDeflectionAbs() < 0.05f && mixerGetRcThrottle() < 0.05f)
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
// or, in altitude hold mode, where throttle can be non-zero
|
||||
|| FLIGHT_MODE(ALT_HOLD_MODE)
|
||||
#endif
|
||||
|
@ -950,7 +950,7 @@ static FAST_CODE_NOINLINE void disarmOnImpact(void)
|
|||
// increase sensitivity by 50% when low and in altitude hold or failsafe landing
|
||||
// for more reliable disarm with gentle controlled landings
|
||||
float lowAltitudeSensitivity = 1.0f;
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
lowAltitudeSensitivity = (FLIGHT_MODE(ALT_HOLD_MODE) && isBelowLandingAltitude()) ? 1.5f : 1.0f;
|
||||
#endif
|
||||
// and disarm if jerk exceeds threshold...
|
||||
|
@ -1123,10 +1123,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
|
|||
float horizonLevelStrength = 0.0f;
|
||||
|
||||
const bool isExternalAngleModeRequest = FLIGHT_MODE(GPS_RESCUE_MODE)
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
|| FLIGHT_MODE(ALT_HOLD_MODE) // todo - check if this is needed
|
||||
#endif
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
|| FLIGHT_MODE(POS_HOLD_MODE)
|
||||
#endif
|
||||
;
|
||||
|
|
|
@ -17,7 +17,7 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
|
||||
#include "math.h"
|
||||
#include "build/debug.h"
|
||||
|
|
|
@ -19,7 +19,7 @@
|
|||
|
||||
// #include "pg/pos_hold.h"
|
||||
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
#include "common/time.h"
|
||||
#include "io/gps.h"
|
||||
|
||||
|
|
|
@ -1805,7 +1805,7 @@ case MSP_NAME:
|
|||
case MSP_RC_DEADBAND:
|
||||
sbufWriteU8(dst, rcControlsConfig()->deadband);
|
||||
sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
sbufWriteU8(dst, posHoldConfig()->pos_hold_deadband);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
|
@ -2965,7 +2965,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
case MSP_SET_RC_DEADBAND:
|
||||
rcControlsConfigMutable()->deadband = sbufReadU8(src);
|
||||
rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
posHoldConfigMutable()->pos_hold_deadband = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
|
|
|
@ -221,10 +221,10 @@ void initActiveBoxIds(void)
|
|||
if (sensors(SENSOR_ACC)) {
|
||||
BME(BOXANGLE);
|
||||
BME(BOXHORIZON);
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
BME(BOXALTHOLD);
|
||||
#endif
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
BME(BOXPOSHOLD);
|
||||
#endif
|
||||
BME(BOXHEADFREE);
|
||||
|
|
|
@ -101,7 +101,7 @@ void sbufWriteBuildInfoFlags(sbuf_t *dst)
|
|||
#ifdef USE_AKK_SMARTAUDIO
|
||||
BUILD_OPTION_AKK_SMARTAUDIO,
|
||||
#endif
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
BUILD_OPTION_ALT_HOLD_MODE,
|
||||
#endif
|
||||
#ifdef USE_BATTERY_CONTINUE
|
||||
|
|
|
@ -249,7 +249,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
|
|||
|
||||
#endif // USE_GPS_RESCUE
|
||||
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
if (osdWarnGetState(OSD_WARNING_POSHOLD_FAILED) && posHoldFailure()) {
|
||||
tfp_sprintf(warningText, "POSHOLD FAIL");
|
||||
*displayAttr = DISPLAYPORT_SEVERITY_WARNING;
|
||||
|
|
|
@ -21,7 +21,7 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
|
||||
#include "flight/alt_hold.h"
|
||||
|
||||
|
|
|
@ -21,7 +21,7 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
|
||||
#include "flight/pos_hold.h"
|
||||
|
||||
|
|
|
@ -119,10 +119,10 @@ typedef enum {
|
|||
#ifdef USE_GPS_RESCUE
|
||||
TASK_GPS_RESCUE,
|
||||
#endif
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#ifdef USE_ALTITUDE_HOLD
|
||||
TASK_ALTHOLD,
|
||||
#endif
|
||||
#ifdef USE_POS_HOLD_MODE
|
||||
#ifdef USE_POSITION_HOLD
|
||||
TASK_POSHOLD,
|
||||
#endif
|
||||
#ifdef USE_MAG
|
||||
|
|
|
@ -245,8 +245,8 @@
|
|||
#define USE_EMFAT_AUTORUN
|
||||
#define USE_EMFAT_ICON
|
||||
#define USE_ESCSERIAL_SIMONK
|
||||
#define USE_ALT_HOLD_MODE
|
||||
#define USE_POS_HOLD_MODE
|
||||
#define USE_ALTITUDE_HOLD
|
||||
#define USE_POSITION_HOLD
|
||||
|
||||
#if !defined(USE_GPS)
|
||||
#define USE_GPS
|
||||
|
|
|
@ -45,7 +45,7 @@ althold_unittest_SRC := \
|
|||
$(USER_DIR)/pg/rx.c
|
||||
|
||||
althold_unittest_DEFINES := \
|
||||
USE_ALT_HOLD_MODE= \
|
||||
USE_ALTITUDE_HOLD= \
|
||||
|
||||
arming_prevention_unittest_SRC := \
|
||||
$(USER_DIR)/common/bitarray.c \
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue