mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
Shared altitude control function in position_control.c (#13954)
* Altitude control code shared fewer debugs subtract D * remove #include comments, simplify coding, restructuring thanks JP and MH * fix cms limits for throttle control * Use altitude_control debug, fix throttle calculation minor refactoring * use AUTO_CONTROL_ALTITUDE debug in place of GPS Rescue throttle pid * use autopilot for position control names * fixes from reviews - thanks * Re-organise included files and functions thanks Karate * missed init and other typos * remove old unused unit test file, tidy up thanks Mark * fix indentation on one line
This commit is contained in:
ctzsnooze
GitHub
parent
1d9823b4cd
commit
58fc8bbbb8
33 changed files with 263 additions and 595 deletions
12
mk/source.mk
12
mk/source.mk
|
|
@ -1,6 +1,7 @@
|
|||
PG_SRC = \
|
||||
pg/adc.c \
|
||||
pg/alt_hold.c \
|
||||
pg/autopilot.c \
|
||||
pg/beeper.c \
|
||||
pg/beeper_dev.c \
|
||||
pg/board.c \
|
||||
|
|
@ -21,7 +22,6 @@ 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 \
|
||||
|
|
@ -152,23 +152,23 @@ COMMON_SRC = \
|
|||
drivers/rx/rx_pwm.c \
|
||||
drivers/serial_softserial.c \
|
||||
fc/core.c \
|
||||
fc/gps_lap_timer.c \
|
||||
fc/rc.c \
|
||||
fc/rc_adjustments.c \
|
||||
fc/rc_controls.c \
|
||||
fc/rc_modes.c \
|
||||
flight/position.c \
|
||||
flight/position_control.c \
|
||||
flight/alt_hold.c \
|
||||
flight/autopilot.c \
|
||||
flight/dyn_notch_filter.c \
|
||||
flight/failsafe.c \
|
||||
flight/gps_rescue.c \
|
||||
fc/gps_lap_timer.c \
|
||||
flight/dyn_notch_filter.c \
|
||||
flight/alt_hold.c \
|
||||
flight/imu.c \
|
||||
flight/mixer.c \
|
||||
flight/mixer_init.c \
|
||||
flight/mixer_tricopter.c \
|
||||
flight/pid.c \
|
||||
flight/pid_init.c \
|
||||
flight/position.c \
|
||||
flight/rpm_filter.c \
|
||||
flight/servos.c \
|
||||
flight/servos_tricopter.c \
|
||||
|
|
|
|||
|
|
@ -63,12 +63,12 @@
|
|||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/alt_hold.h"
|
||||
#include "flight/autopilot.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/gps_rescue.h"
|
||||
#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"
|
||||
|
||||
|
|
@ -1616,12 +1616,15 @@ 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", 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);
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", autopilotConfig()->landing_altitude_m);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", autopilotConfig()->hover_throttle);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d", autopilotConfig()->throttle_min);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d", autopilotConfig()->throttle_max);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d", autopilotConfig()->altitude_P);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d", autopilotConfig()->altitude_I);;
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d", autopilotConfig()->altitude_D);;
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d", autopilotConfig()->altitude_F);
|
||||
|
||||
#ifdef USE_MAG
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware);
|
||||
|
|
@ -1713,9 +1716,6 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold)
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, "%d", gpsRescueConfig()->throttleMin)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, "%d", gpsRescueConfig()->throttleMax)
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks)
|
||||
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)
|
||||
|
|
@ -1732,8 +1732,6 @@ static bool blackboxWriteSysinfo(void)
|
|||
#endif // USE_GPS
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
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);
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -83,7 +83,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
[DEBUG_CRSF_LINK_STATISTICS_PWR] = "CRSF_LINK_STATISTICS_PWR",
|
||||
[DEBUG_CRSF_LINK_STATISTICS_DOWN] = "CRSF_LINK_STATISTICS_DOWN",
|
||||
[DEBUG_BARO] = "BARO",
|
||||
[DEBUG_GPS_RESCUE_THROTTLE_PID] = "GPS_RESCUE_THROTTLE_PID",
|
||||
[DEBUG_AUTOPILOT_ALTITUDE] = "AUTOPILOT_ALTITUDE",
|
||||
[DEBUG_DYN_IDLE] = "DYN_IDLE",
|
||||
[DEBUG_FEEDFORWARD_LIMIT] = "FEEDFORWARD_LIMIT",
|
||||
[DEBUG_FEEDFORWARD] = "FEEDFORWARD",
|
||||
|
|
@ -121,5 +121,4 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
[DEBUG_S_TERM] = "S_TERM",
|
||||
[DEBUG_SPA] = "SPA",
|
||||
[DEBUG_TASK] = "TASK",
|
||||
[DEBUG_ALTHOLD] = "ALTHOLD",
|
||||
};
|
||||
|
|
|
|||
|
|
@ -85,7 +85,7 @@ typedef enum {
|
|||
DEBUG_CRSF_LINK_STATISTICS_PWR,
|
||||
DEBUG_CRSF_LINK_STATISTICS_DOWN,
|
||||
DEBUG_BARO,
|
||||
DEBUG_GPS_RESCUE_THROTTLE_PID,
|
||||
DEBUG_AUTOPILOT_ALTITUDE,
|
||||
DEBUG_DYN_IDLE,
|
||||
DEBUG_FEEDFORWARD_LIMIT,
|
||||
DEBUG_FEEDFORWARD,
|
||||
|
|
@ -123,7 +123,6 @@ typedef enum {
|
|||
DEBUG_S_TERM,
|
||||
DEBUG_SPA,
|
||||
DEBUG_TASK,
|
||||
DEBUG_ALTHOLD,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -104,12 +104,12 @@ bool cliMode = false;
|
|||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/autopilot.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#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"
|
||||
|
|
|
|||
|
|
@ -56,14 +56,14 @@
|
|||
#include "fc/rc_adjustments.h"
|
||||
#include "fc/rc_controls.h"
|
||||
|
||||
#include "flight/alt_hold.h"
|
||||
#include "flight/autopilot.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/gps_rescue.h"
|
||||
#include "flight/imu.h"
|
||||
#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"
|
||||
|
||||
|
|
@ -1071,17 +1071,14 @@ const clivalue_t valueTable[] = {
|
|||
{ PARAM_NAME_GPS_RESCUE_DESCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 25, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) },
|
||||
{ PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 250 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, disarmThreshold) },
|
||||
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) },
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) },
|
||||
|
||||
{ PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_SANITY_CHECK }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) },
|
||||
{ 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_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) },
|
||||
{ PARAM_NAME_GPS_RESCUE_YAW_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, yawP) },
|
||||
{ PARAM_NAME_GPS_RESCUE_VELOCITY_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velP) },
|
||||
{ PARAM_NAME_GPS_RESCUE_VELOCITY_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velI) },
|
||||
{ PARAM_NAME_GPS_RESCUE_VELOCITY_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velD) },
|
||||
{ PARAM_NAME_GPS_RESCUE_YAW_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, yawP) },
|
||||
|
||||
#ifdef USE_MAG
|
||||
{ PARAM_NAME_GPS_RESCUE_USE_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
|
||||
|
|
@ -1102,8 +1099,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_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) },
|
||||
#endif
|
||||
|
||||
|
|
@ -1835,13 +1830,15 @@ const clivalue_t valueTable[] = {
|
|||
{ 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) },
|
||||
|
||||
// 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_AUTOPILOT
|
||||
{ PARAM_NAME_LANDING_ALTITUDE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, landing_altitude_m) },
|
||||
{ PARAM_NAME_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, hover_throttle) },
|
||||
{ PARAM_NAME_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, throttle_min) },
|
||||
{ PARAM_NAME_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1400, 2000 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, throttle_max) },
|
||||
{ PARAM_NAME_ALTITUDE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_P) },
|
||||
{ PARAM_NAME_ALTITUDE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_I) },
|
||||
{ PARAM_NAME_ALTITUDE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_D) },
|
||||
{ PARAM_NAME_ALTITUDE_F, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_F) },
|
||||
|
||||
// PG_MODE_ACTIVATION_CONFIG
|
||||
#if defined(USE_CUSTOM_BOX_NAMES)
|
||||
|
|
|
|||
|
|
@ -37,9 +37,9 @@
|
|||
|
||||
#include "config/config.h"
|
||||
|
||||
#include "flight/autopilot.h"
|
||||
#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;
|
||||
|
|
@ -52,16 +52,16 @@ static uint8_t gpsRescueConfig_angle; //degrees
|
|||
|
||||
static uint16_t gpsRescueConfig_descentDistanceM; //meters
|
||||
static uint16_t gpsRescueConfig_descendRate;
|
||||
static uint8_t positionControlConfig_landingAltitudeM;
|
||||
static uint8_t autopilotConfig_landingAltitudeM;
|
||||
|
||||
static uint16_t gpsRescueConfig_throttleMin;
|
||||
static uint16_t gpsRescueConfig_throttleMax;
|
||||
static uint16_t positionControlConfig_hoverThrottle;
|
||||
static uint16_t autopilotConfig_throttleMin;
|
||||
static uint16_t autopilotConfig_throttleMax;
|
||||
static uint16_t autopilotConfig_hoverThrottle;
|
||||
|
||||
static uint8_t gpsRescueConfig_minSats;
|
||||
static uint8_t gpsRescueConfig_allowArmingWithoutFix;
|
||||
|
||||
static uint8_t positionControlConfig_altitude_P, positionControlConfig_altitude_I, positionControlConfig_altitude_D, positionControlConfig_altitude_F;
|
||||
static uint8_t autopilotConfig_altitude_P, autopilotConfig_altitude_I, autopilotConfig_altitude_D, autopilotConfig_altitude_F;
|
||||
static uint8_t gpsRescueConfig_yawP;
|
||||
static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
|
||||
|
||||
|
|
@ -72,10 +72,10 @@ static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
|
|||
{
|
||||
UNUSED(pDisp);
|
||||
|
||||
positionControlConfig_altitude_P = positionControlConfig()->altitude_P;
|
||||
positionControlConfig_altitude_I = positionControlConfig()->altitude_I;
|
||||
positionControlConfig_altitude_D = positionControlConfig()->altitude_D;
|
||||
positionControlConfig_altitude_F = positionControlConfig()->altitude_F;
|
||||
autopilotConfig_altitude_P = autopilotConfig()->altitude_P;
|
||||
autopilotConfig_altitude_I = autopilotConfig()->altitude_I;
|
||||
autopilotConfig_altitude_D = autopilotConfig()->altitude_D;
|
||||
autopilotConfig_altitude_F = autopilotConfig()->altitude_F;
|
||||
|
||||
gpsRescueConfig_yawP = gpsRescueConfig()->yawP;
|
||||
|
||||
|
|
@ -94,10 +94,10 @@ static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_En
|
|||
UNUSED(pDisp);
|
||||
UNUSED(self);
|
||||
|
||||
positionControlConfigMutable()->altitude_P = positionControlConfig_altitude_P;
|
||||
positionControlConfigMutable()->altitude_I = positionControlConfig_altitude_I;
|
||||
positionControlConfigMutable()->altitude_D = positionControlConfig_altitude_D;
|
||||
positionControlConfigMutable()->altitude_F = positionControlConfig_altitude_F;
|
||||
autopilotConfigMutable()->altitude_P = autopilotConfig_altitude_P;
|
||||
autopilotConfigMutable()->altitude_I = autopilotConfig_altitude_I;
|
||||
autopilotConfigMutable()->altitude_D = autopilotConfig_altitude_D;
|
||||
autopilotConfigMutable()->altitude_F = autopilotConfig_altitude_F;
|
||||
|
||||
gpsRescueConfigMutable()->yawP = gpsRescueConfig_yawP;
|
||||
|
||||
|
|
@ -115,16 +115,16 @@ const OSD_Entry cms_menuGpsRescuePidEntries[] =
|
|||
{
|
||||
{"--- GPS RESCUE PID---", OME_Label, NULL, NULL},
|
||||
|
||||
{ "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 } },
|
||||
{ "ALTITUDE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_P, 0, 200, 1 } },
|
||||
{ "ALTITUDE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_I, 0, 200, 1 } },
|
||||
{ "ALTITUDE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_D, 0, 200, 1 } },
|
||||
{ "ALTITUDE F", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_F, 0, 200, 1 } },
|
||||
|
||||
{ "YAW P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_yawP, 0, 255, 1 } },
|
||||
{ "YAW P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_yawP, 0, 200, 1 } },
|
||||
|
||||
{ "VELOCITY P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velP, 0, 255, 1 } },
|
||||
{ "VELOCITY I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velI, 0, 255, 1 } },
|
||||
{ "VELOCITY D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velD, 0, 255, 1 } },
|
||||
{ "VELOCITY P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velP, 0, 200, 1 } },
|
||||
{ "VELOCITY I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velI, 0, 200, 1 } },
|
||||
{ "VELOCITY D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velD, 0, 200, 1 } },
|
||||
|
||||
{ "SMOOTHING", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_pitchCutoffHz, 10, 255, 1 } },
|
||||
{ "IMU_YAW_GAIN", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_imuYawGain, 5, 20, 1 } },
|
||||
|
|
@ -159,11 +159,11 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
|
|||
|
||||
gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
|
||||
gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
|
||||
positionControlConfig_landingAltitudeM = positionControlConfig()->landing_altitude_m;
|
||||
autopilotConfig_landingAltitudeM = autopilotConfig()->landing_altitude_m;
|
||||
|
||||
gpsRescueConfig_throttleMin = gpsRescueConfig()->throttleMin;
|
||||
gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax;
|
||||
positionControlConfig_hoverThrottle = positionControlConfig()->hover_throttle;
|
||||
autopilotConfig_throttleMin = autopilotConfig()->throttle_min;
|
||||
autopilotConfig_throttleMax = autopilotConfig()->throttle_max;
|
||||
autopilotConfig_hoverThrottle = autopilotConfig()->hover_throttle;
|
||||
|
||||
gpsRescueConfig_minSats = gpsRescueConfig()->minSats;
|
||||
gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix;
|
||||
|
|
@ -187,11 +187,11 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
|
|||
|
||||
gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
|
||||
gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;
|
||||
positionControlConfigMutable()->landing_altitude_m = positionControlConfig_landingAltitudeM;
|
||||
autopilotConfigMutable()->landing_altitude_m = autopilotConfig_landingAltitudeM;
|
||||
|
||||
gpsRescueConfigMutable()->throttleMin = gpsRescueConfig_throttleMin;
|
||||
gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax;
|
||||
positionControlConfigMutable()->hover_throttle = positionControlConfig_hoverThrottle;
|
||||
autopilotConfigMutable()->throttle_min = autopilotConfig_throttleMin;
|
||||
autopilotConfigMutable()->throttle_max = autopilotConfig_throttleMax;
|
||||
autopilotConfigMutable()->hover_throttle = autopilotConfig_hoverThrottle;
|
||||
|
||||
gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats;
|
||||
gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix;
|
||||
|
|
@ -214,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){ &positionControlConfig_landingAltitudeM, 1, 15, 1 } },
|
||||
{ "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_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){ &positionControlConfig_hoverThrottle, 1100, 1700, 1 } },
|
||||
{ "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_throttleMin, 1050, 1400, 1 } },
|
||||
{ "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_throttleMax, 1400, 2000, 1 } },
|
||||
{ "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_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 },
|
||||
|
|
|
|||
|
|
@ -95,6 +95,7 @@
|
|||
#include "fc/tasks.h"
|
||||
|
||||
#include "flight/alt_hold.h"
|
||||
#include "flight/autopilot.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
|
|
@ -102,7 +103,6 @@
|
|||
#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"
|
||||
|
|
@ -828,7 +828,7 @@ void init(void)
|
|||
#endif
|
||||
|
||||
positionInit();
|
||||
positionControlInit(positionControlConfig());
|
||||
autopilotInit(autopilotConfig());
|
||||
|
||||
#if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL)
|
||||
vtxTableInit();
|
||||
|
|
@ -1000,7 +1000,7 @@ void init(void)
|
|||
spiInitBusDMA();
|
||||
#endif
|
||||
|
||||
// position_control must be initialised before modes that require the position_control pids
|
||||
// autopilot must be initialised before modes that require the autopilot pids
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
altHoldInit();
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -158,12 +158,15 @@
|
|||
#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_THROTTLE_MIN "autopilot_throttle_min"
|
||||
#define PARAM_NAME_THROTTLE_MAX "autopilot_throttle_max"
|
||||
#define PARAM_NAME_ALTITUDE_P "autopilot_altitude_P"
|
||||
#define PARAM_NAME_ALTITUDE_I "autopilot_altitude_I"
|
||||
#define PARAM_NAME_ALTITUDE_D "autopilot_altitude_D"
|
||||
#define PARAM_NAME_ALTITUDE_F "autopilot_altitude_F"
|
||||
|
||||
#define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward"
|
||||
#define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms"
|
||||
|
|
@ -212,9 +215,6 @@
|
|||
#define PARAM_NAME_GPS_RESCUE_DESCEND_RATE "gps_rescue_descend_rate"
|
||||
#define PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD "gps_rescue_disarm_threshold"
|
||||
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_MIN "gps_rescue_throttle_min"
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_MAX "gps_rescue_throttle_max"
|
||||
|
||||
#define PARAM_NAME_GPS_RESCUE_SANITY_CHECKS "gps_rescue_sanity_checks"
|
||||
#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"
|
||||
|
|
@ -240,8 +240,6 @@
|
|||
#endif // USE_GPS
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
#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"
|
||||
#endif // USE_ALT_HOLD_MODE
|
||||
|
||||
|
|
|
|||
|
|
@ -16,124 +16,52 @@
|
|||
*/
|
||||
|
||||
#include "platform.h"
|
||||
#include "math.h"
|
||||
|
||||
#ifdef USE_ALT_HOLD_MODE
|
||||
|
||||
#include "math.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "config/config.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "fc/rc.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "flight/autopilot.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"
|
||||
|
||||
static float pidIntegral = 0.0f;
|
||||
static const float taskIntervalSeconds = HZ_TO_INTERVAL(ALTHOLD_TASK_RATE_HZ); // i.e. 0.01 s
|
||||
|
||||
altHoldState_t altHoldState;
|
||||
|
||||
float altitudePidCalculate(void)
|
||||
void controlAltitude(void)
|
||||
{
|
||||
// * introductory notes *
|
||||
// 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:
|
||||
// -reduce P I and D by 1/3 or until it doesn't oscillate but has sloppy / slow control
|
||||
// increase P until there is definite oscillation, then back off until barely noticeable
|
||||
// increase D until there is definite oscillation (will be faster than P), then back off until barely noticeable
|
||||
// try to add a little more P, then try to add a little more D, but not to oscillation
|
||||
// iTerm isn't very important if hover throttle is set carefully and sag compensation is used
|
||||
|
||||
// The altitude D lowpass filter is very important.
|
||||
// The only way to get enough D is to filter the oscillations out.
|
||||
// More D filtering is needed with Baro than with GPS, since GPS is smoother and slower.
|
||||
|
||||
// A major problem is the lag time for motors to arrest pre-existing drops or climbs,
|
||||
// compounded by the lag time from filtering.
|
||||
// If the quad is dropping fast, the motors have to be high for a long time to arrest the drop
|
||||
// this is very difficult for a 'simple' PID controller;
|
||||
// if the PIDs are high enough to arrest a fast drop, they will oscillate under normal conditions
|
||||
// Hence we:
|
||||
// - Enhance D when the absolute velocity is high, ie when we need to strongly oppose a fast drop,
|
||||
// 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 pidCoefficient_t *pidCoefs = getAltitudePidCoeffs();
|
||||
|
||||
const float altitudeErrorCm = altHoldState.targetAltitudeCm - getAltitudeCm();
|
||||
|
||||
// P
|
||||
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
|
||||
|
||||
// much less iTerm change for errors greater than 2m, otherwise it winds up badly
|
||||
const float itermNormalRange = 200.0f; // 2m
|
||||
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 **
|
||||
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)
|
||||
// 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 = getAltitudeDerivative(); // cm/s altitude derivative is always available
|
||||
float verticalVelocity = 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;
|
||||
if (fabsf(vel) > kinkPoint) {
|
||||
vel = vel * 3.0f - sign * kinkPointAdjustment;
|
||||
const float sign = (verticalVelocity > 0) ? 1.0f : -1.0f;
|
||||
if (fabsf(verticalVelocity) > kinkPoint) {
|
||||
verticalVelocity = verticalVelocity * 3.0f - sign * kinkPointAdjustment;
|
||||
}
|
||||
|
||||
const float dOut = pidCoefs->Kd * vel;
|
||||
|
||||
// F
|
||||
// if error is used, we get a 'free kick' in derivative from changes in the target value
|
||||
// but this is delayed by the smoothing, leading to lag and overshoot.
|
||||
// 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 = 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;
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 4, lrintf(pOut));
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 5, lrintf(iOut));
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 6, lrintf(dOut));
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 7, lrintf(fOut));
|
||||
|
||||
return output; // motor units, eg 100 means 10% of available throttle
|
||||
//run the function in autopilot.c that calculates the PIDs and drives the motors
|
||||
altitudeControl(altHoldState.targetAltitudeCm, taskIntervalSeconds, verticalVelocity, altHoldState.targetAltitudeAdjustRate);
|
||||
}
|
||||
|
||||
void altHoldReset(void)
|
||||
{
|
||||
pidIntegral = 0.0f;
|
||||
resetAltitudeControl();
|
||||
altHoldState.targetAltitudeCm = getAltitudeCm();
|
||||
altHoldState.targetAltitudeAdjustRate = 0.0f;
|
||||
}
|
||||
|
||||
void altHoldInit(void)
|
||||
{
|
||||
altHoldState.hoverThrottle = positionControlConfig()->hover_throttle - PWM_RANGE_MIN;
|
||||
altHoldState.isAltHoldActive = false;
|
||||
altHoldReset();
|
||||
}
|
||||
|
|
@ -177,8 +105,8 @@ void altHoldUpdateTargetAltitude(void)
|
|||
|
||||
const float rcThrottle = rcCommand[THROTTLE];
|
||||
|
||||
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
|
||||
const float lowThreshold = 0.5f * (autopilotConfig()->hover_throttle + PWM_RANGE_MIN); // halfway between hover and MIN, e.g. 1150 if hover is 1300
|
||||
const float highThreshold = 0.5f * (autopilotConfig()->hover_throttle + PWM_RANGE_MAX); // halfway between hover and MAX, e.g. 1650 if hover is 1300
|
||||
|
||||
float throttleAdjustmentFactor = 0.0f;
|
||||
if (rcThrottle < lowThreshold) {
|
||||
|
|
@ -186,7 +114,6 @@ void altHoldUpdateTargetAltitude(void)
|
|||
} else if (rcThrottle > highThreshold) {
|
||||
throttleAdjustmentFactor = scaleRangef(rcThrottle, highThreshold, PWM_RANGE_MAX, 0.0f, 1.0f);
|
||||
}
|
||||
|
||||
// if failsafe is active, and we get here, we are in failsafe landing mode
|
||||
if (failsafeIsActive()) {
|
||||
// descend at up to 10 times faster when high
|
||||
|
|
@ -198,8 +125,8 @@ void altHoldUpdateTargetAltitude(void)
|
|||
// constant (set) deceleration target in the last 2m
|
||||
throttleAdjustmentFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f));
|
||||
}
|
||||
|
||||
altHoldState.targetAltitudeAdjustRate = throttleAdjustmentFactor * altholdConfig()->alt_hold_target_adjust_rate;
|
||||
|
||||
// if taskRate is 100Hz, default adjustRate of 100 adds/subtracts 1m every second, or 1cm per task run, at full stick position
|
||||
altHoldState.targetAltitudeCm += altHoldState.targetAltitudeAdjustRate * taskIntervalSeconds;
|
||||
}
|
||||
|
|
@ -211,18 +138,7 @@ void altHoldUpdate(void)
|
|||
altHoldUpdateTargetAltitude();
|
||||
}
|
||||
|
||||
// use PIDs to return the throttle adjustment value, add it to the hover value, and constrain
|
||||
const float throttleAdjustment = altitudePidCalculate();
|
||||
|
||||
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.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(newThrottle)); // normal range 1000-2000, but is beforeconstraint
|
||||
DEBUG_SET(DEBUG_ALTHOLD, 3, lrintf(tiltMultiplier * 100));
|
||||
controlAltitude();
|
||||
}
|
||||
|
||||
void updateAltHoldState(timeUs_t currentTimeUs) {
|
||||
|
|
@ -235,13 +151,4 @@ void updateAltHoldState(timeUs_t currentTimeUs) {
|
|||
altHoldUpdate();
|
||||
}
|
||||
}
|
||||
|
||||
float altHoldGetThrottle(void) {
|
||||
// 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,12 +28,9 @@ typedef struct {
|
|||
bool isAltHoldActive;
|
||||
float targetAltitudeCm;
|
||||
float targetAltitudeAdjustRate;
|
||||
float throttleOut;
|
||||
float hoverThrottle;
|
||||
} altHoldState_t;
|
||||
|
||||
void altHoldInit(void);
|
||||
void updateAltHoldState(timeUs_t currentTimeUs);
|
||||
float altHoldGetThrottle(void);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
107
src/main/flight/autopilot.c
Normal file
107
src/main/flight/autopilot.c
Normal file
|
|
@ -0,0 +1,107 @@
|
|||
/*
|
||||
* 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 <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "build/debug.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "flight/imu.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "autopilot.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;
|
||||
static float altitudeI = 0.0f;
|
||||
static float throttleOut = 0.0f;
|
||||
|
||||
void autopilotInit(const autopilotConfig_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;
|
||||
}
|
||||
|
||||
void resetAltitudeControl (void) {
|
||||
altitudeI = 0.0f;
|
||||
}
|
||||
|
||||
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep) {
|
||||
|
||||
const float altitudeErrorCm = targetAltitudeCm - getAltitudeCm();
|
||||
const float altitudeP = altitudeErrorCm * altitudePidCoeffs.Kp;
|
||||
|
||||
// reduce the iTerm gain for errors greater than 200cm (2m), otherwise it winds up too much
|
||||
const float itermRelax = (fabsf(altitudeErrorCm) < 200.0f) ? 1.0f : 0.1f;
|
||||
altitudeI += altitudeErrorCm * altitudePidCoeffs.Ki * itermRelax * taskIntervalS;
|
||||
// limit iTerm to not more than 200 throttle units
|
||||
altitudeI = constrainf(altitudeI, -200.0f, 200.0f);
|
||||
|
||||
const float altitudeD = verticalVelocity * altitudePidCoeffs.Kd;
|
||||
|
||||
const float altitudeF = targetAltitudeStep * altitudePidCoeffs.Kf;
|
||||
|
||||
const float hoverOffset = autopilotConfig()->hover_throttle - PWM_RANGE_MIN;
|
||||
|
||||
float throttleOffset = altitudeP + altitudeI - altitudeD + altitudeF + hoverOffset;
|
||||
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
|
||||
throttleOffset *= tiltMultiplier;
|
||||
|
||||
float newThrottle = PWM_RANGE_MIN + throttleOffset;
|
||||
newThrottle = constrainf(newThrottle, autopilotConfig()->throttle_min, autopilotConfig()->throttle_max);
|
||||
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 0, lrintf(newThrottle)); // normal range 1000-2000 but is before constraint
|
||||
|
||||
newThrottle = scaleRangef(newThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
|
||||
|
||||
throttleOut = constrainf(newThrottle, 0.0f, 1.0f);
|
||||
|
||||
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 1, lrintf(tiltMultiplier * 100));
|
||||
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 3, lrintf(targetAltitudeCm));
|
||||
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 4, lrintf(altitudeP));
|
||||
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 5, lrintf(altitudeI));
|
||||
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 6, lrintf(-altitudeD));
|
||||
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 7, lrintf(altitudeF));
|
||||
}
|
||||
|
||||
bool isBelowLandingAltitude(void)
|
||||
{
|
||||
return getAltitudeCm() < 100.0f * autopilotConfig()->landing_altitude_m;
|
||||
}
|
||||
|
||||
const pidCoefficient_t *getAltitudePidCoeffs(void)
|
||||
{
|
||||
return &altitudePidCoeffs;
|
||||
}
|
||||
|
||||
float getAutopilotThrottle(void)
|
||||
{
|
||||
return throttleOut;
|
||||
}
|
||||
|
|
@ -17,11 +17,14 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "pg/position_control.h"
|
||||
#include "pg/autopilot.h"
|
||||
#include "flight/pid.h"
|
||||
|
||||
void positionControlInit(const positionControlConfig_t *config);
|
||||
void autopilotInit(const autopilotConfig_t *config);
|
||||
void resetAltitudeControl(void);
|
||||
|
||||
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep);
|
||||
|
||||
bool isBelowLandingAltitude(void);
|
||||
|
||||
const pidCoefficient_t *getAltitudePidCoeffs(void);
|
||||
float getAutopilotThrottle(void);
|
||||
|
|
@ -30,24 +30,22 @@
|
|||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "config/config.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
||||
#include "config/config.h"
|
||||
#include "fc/core.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/autopilot.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/acceleration.h"
|
||||
|
||||
#include "gps_rescue.h"
|
||||
|
|
@ -208,8 +206,6 @@ static void rescueAttainPosition(void)
|
|||
// runs at 100hz, but only updates RPYT settings when new GPS Data arrives and when not in idle phase.
|
||||
static float previousVelocityError = 0.0f;
|
||||
static float velocityI = 0.0f;
|
||||
static float throttleI = 0.0f;
|
||||
|
||||
switch (rescueState.phase) {
|
||||
case RESCUE_IDLE:
|
||||
// values to be returned when no rescue is active
|
||||
|
|
@ -221,7 +217,7 @@ static void rescueAttainPosition(void)
|
|||
// Initialize internal variables each time GPS Rescue is started
|
||||
previousVelocityError = 0.0f;
|
||||
velocityI = 0.0f;
|
||||
throttleI = 0.0f;
|
||||
resetAltitudeControl();
|
||||
rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold * 0.1f;
|
||||
rescueState.sensor.imuYawCogGain = 1.0f;
|
||||
return;
|
||||
|
|
@ -229,7 +225,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 = positionControlConfig()->hover_throttle - 100;
|
||||
rescueThrottle = autopilotConfig()->hover_throttle - 100;
|
||||
return;
|
||||
default:
|
||||
break;
|
||||
|
|
@ -238,48 +234,9 @@ static void rescueAttainPosition(void)
|
|||
/**
|
||||
Altitude (throttle) controller
|
||||
*/
|
||||
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 = getAltitudePidCoeffs()->Kp * altitudeErrorCm;
|
||||
|
||||
// I component
|
||||
// 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, need to check if this is needed, in practice.
|
||||
|
||||
// D component
|
||||
const float throttleD = getAltitudeDerivative() * getAltitudePidCoeffs()->Kd * rescueState.intent.throttleDMultiplier;
|
||||
|
||||
// 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;
|
||||
|
||||
const float hoverOffset = positionControlConfig()->hover_throttle - PWM_RANGE_MIN;
|
||||
|
||||
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 = 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, 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
|
||||
const float verticalVelocity = getAltitudeDerivative() * rescueState.intent.throttleDMultiplier;
|
||||
altitudeControl(rescueState.intent.targetAltitudeCm, taskIntervalSeconds, verticalVelocity, rescueState.intent.targetAltitudeStepCm);
|
||||
|
||||
/**
|
||||
Heading / yaw controller
|
||||
|
|
@ -687,7 +644,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 * positionControlConfig()->landing_altitude_m);
|
||||
const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * autopilotConfig()->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
|
||||
|
|
@ -918,7 +875,6 @@ void gpsRescueUpdate(void)
|
|||
}
|
||||
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 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();
|
||||
|
|
@ -937,20 +893,6 @@ float gpsRescueGetImuYawCogGain(void)
|
|||
return rescueState.sensor.imuYawCogGain;
|
||||
}
|
||||
|
||||
float gpsRescueGetThrottle(void)
|
||||
{
|
||||
// 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;
|
||||
}
|
||||
|
||||
bool gpsRescueIsConfigured(void)
|
||||
{
|
||||
return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE || isModeActivationConditionPresent(BOXGPSRESCUE);
|
||||
|
|
|
|||
|
|
@ -51,7 +51,6 @@ void gpsRescueInit(void);
|
|||
void gpsRescueUpdate(void);
|
||||
void gpsRescueNewGpsData(void);
|
||||
float gpsRescueGetYawRate(void);
|
||||
float gpsRescueGetThrottle(void);
|
||||
bool gpsRescueIsConfigured(void);
|
||||
bool gpsRescueIsAvailable(void);
|
||||
bool gpsRescueIsDisabled(void);
|
||||
|
|
|
|||
|
|
@ -45,8 +45,9 @@
|
|||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/alt_hold.h"
|
||||
#include "flight/autopilot.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/gps_rescue.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer_init.h"
|
||||
|
|
@ -801,7 +802,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
|
|||
#ifdef USE_ALT_HOLD_MODE
|
||||
// Throttle value to be used during altitude hold mode (and failsafe landing mode)
|
||||
if (FLIGHT_MODE(ALT_HOLD_MODE)) {
|
||||
throttle = altHoldGetThrottle();
|
||||
throttle = getAutopilotThrottle();
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -809,7 +810,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
|
|||
// If gps rescue is active then override the throttle. This prevents things
|
||||
// like throttle boost or throttle limit from negatively affecting the throttle.
|
||||
if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
|
||||
throttle = gpsRescueGetThrottle();
|
||||
throttle = getAutopilotThrottle();
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -47,10 +47,10 @@
|
|||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/alt_hold.h"
|
||||
#include "flight/autopilot.h"
|
||||
#include "flight/gps_rescue.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/rpm_filter.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
|
|
|||
|
|
@ -34,7 +34,6 @@
|
|||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/pid.h"
|
||||
|
||||
|
|
@ -211,8 +210,7 @@ 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));
|
||||
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 2, lrintf(zeroedAltitudeCm));
|
||||
|
||||
altitudeAvailable = haveGpsAlt || haveBaroAlt;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,50 +0,0 @@
|
|||
/*
|
||||
* 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;
|
||||
}
|
||||
|
|
@ -82,6 +82,7 @@
|
|||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/autopilot.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/gps_rescue.h"
|
||||
#include "flight/imu.h"
|
||||
|
|
@ -89,7 +90,6 @@
|
|||
#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"
|
||||
|
||||
|
|
@ -1544,9 +1544,9 @@ case MSP_NAME:
|
|||
sbufWriteU16(dst, gpsRescueConfig()->returnAltitudeM);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->groundSpeedCmS);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->throttleMin);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->throttleMax);
|
||||
sbufWriteU16(dst, positionControlConfig()->hover_throttle);
|
||||
sbufWriteU16(dst, autopilotConfig()->throttle_min);
|
||||
sbufWriteU16(dst, autopilotConfig()->throttle_max);
|
||||
sbufWriteU16(dst, autopilotConfig()->hover_throttle);
|
||||
sbufWriteU8(dst, gpsRescueConfig()->sanityChecks);
|
||||
sbufWriteU8(dst, gpsRescueConfig()->minSats);
|
||||
|
||||
|
|
@ -1562,9 +1562,9 @@ case MSP_NAME:
|
|||
break;
|
||||
|
||||
case MSP_GPS_RESCUE_PIDS:
|
||||
sbufWriteU16(dst, positionControlConfig()->altitude_P);
|
||||
sbufWriteU16(dst, positionControlConfig()->altitude_I);
|
||||
sbufWriteU16(dst, positionControlConfig()->altitude_D);
|
||||
sbufWriteU16(dst, autopilotConfig()->altitude_P);
|
||||
sbufWriteU16(dst, autopilotConfig()->altitude_I);
|
||||
sbufWriteU16(dst, autopilotConfig()->altitude_D);
|
||||
// altitude_F not implemented yet
|
||||
sbufWriteU16(dst, gpsRescueConfig()->velP);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->velI);
|
||||
|
|
@ -2879,9 +2879,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
gpsRescueConfigMutable()->returnAltitudeM = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->groundSpeedCmS = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->throttleMin = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->throttleMax = sbufReadU16(src);
|
||||
positionControlConfigMutable()->hover_throttle = sbufReadU16(src);
|
||||
autopilotConfigMutable()->throttle_min = sbufReadU16(src);
|
||||
autopilotConfigMutable()->throttle_max = sbufReadU16(src);
|
||||
autopilotConfigMutable()->hover_throttle = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src);
|
||||
gpsRescueConfigMutable()->minSats = sbufReadU8(src);
|
||||
if (sbufBytesRemaining(src) >= 6) {
|
||||
|
|
@ -2902,9 +2902,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
break;
|
||||
|
||||
case MSP_SET_GPS_RESCUE_PIDS:
|
||||
positionControlConfigMutable()->altitude_P = sbufReadU16(src);
|
||||
positionControlConfigMutable()->altitude_I = sbufReadU16(src);
|
||||
positionControlConfigMutable()->altitude_D = sbufReadU16(src);
|
||||
autopilotConfigMutable()->altitude_P = sbufReadU16(src);
|
||||
autopilotConfigMutable()->altitude_I = sbufReadU16(src);
|
||||
autopilotConfigMutable()->altitude_D = sbufReadU16(src);
|
||||
// altitude_F not included in msp yet
|
||||
gpsRescueConfigMutable()->velP = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->velI = sbufReadU16(src);
|
||||
|
|
|
|||
|
|
@ -33,7 +33,5 @@ PG_REGISTER_WITH_RESET_TEMPLATE(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFI
|
|||
|
||||
PG_RESET_TEMPLATE(altholdConfig_t, altholdConfig,
|
||||
.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,
|
||||
);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -26,8 +26,6 @@
|
|||
|
||||
typedef struct altholdConfig_s {
|
||||
uint8_t alt_hold_target_adjust_rate;
|
||||
uint16_t alt_hold_throttle_min;
|
||||
uint16_t alt_hold_throttle_max;
|
||||
} altholdConfig_t;
|
||||
|
||||
PG_DECLARE(altholdConfig_t, altholdConfig);
|
||||
|
|
|
|||
|
|
@ -20,18 +20,20 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/autopilot.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
#include "position_control.h"
|
||||
#include "autopilot.h"
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 1);
|
||||
|
||||
PG_RESET_TEMPLATE(positionControlConfig_t, positionControlConfig,
|
||||
.hover_throttle = 1275,
|
||||
PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig,
|
||||
.landing_altitude_m = 4,
|
||||
.hover_throttle = 1275,
|
||||
.throttle_min = 1100,
|
||||
.throttle_max = 1700,
|
||||
.altitude_P = 15,
|
||||
.altitude_I = 15,
|
||||
.altitude_D = 15,
|
||||
|
|
@ -24,14 +24,16 @@
|
|||
|
||||
#include "pg/pg.h"
|
||||
|
||||
typedef struct positionControlConfig_s {
|
||||
uint16_t hover_throttle; // value used at the start of a rescue or position hold
|
||||
typedef struct autopilotConfig_s {
|
||||
uint8_t landing_altitude_m; // altitude below which landing behaviours can change, metres
|
||||
uint16_t hover_throttle; // value used at the start of a rescue or position hold
|
||||
uint16_t throttle_min;
|
||||
uint16_t throttle_max;
|
||||
uint8_t altitude_P;
|
||||
uint8_t altitude_I;
|
||||
uint8_t altitude_D;
|
||||
uint8_t altitude_F;
|
||||
} positionControlConfig_t;
|
||||
} autopilotConfig_t;
|
||||
|
||||
PG_DECLARE(positionControlConfig_t, positionControlConfig);
|
||||
PG_DECLARE(autopilotConfig_t, autopilotConfig);
|
||||
|
||||
|
|
@ -48,9 +48,6 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
|||
.descendRate = 150, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
|
||||
.disarmThreshold = 30,
|
||||
|
||||
.throttleMin = 1100,
|
||||
.throttleMax = 1700,
|
||||
|
||||
.allowArmingWithoutFix = false,
|
||||
.sanityChecks = RESCUE_SANITY_FS_ONLY,
|
||||
.minSats = 8,
|
||||
|
|
|
|||
|
|
@ -31,8 +31,6 @@ typedef struct gpsRescue_s {
|
|||
uint16_t descentDistanceM; // meters
|
||||
uint16_t groundSpeedCmS; // centimeters per second
|
||||
uint8_t yawP;
|
||||
uint16_t throttleMin;
|
||||
uint16_t throttleMax;
|
||||
uint8_t minSats;
|
||||
uint8_t velP, velI, velD;
|
||||
uint16_t minStartDistM; // meters
|
||||
|
|
|
|||
|
|
@ -83,7 +83,7 @@
|
|||
#define PG_VTX_IO_CONFIG 57
|
||||
#define PG_GPS_LAP_TIMER 58
|
||||
#define PG_ALTHOLD_CONFIG 59
|
||||
#define PG_POSITION_CONTROL 60
|
||||
#define PG_AUTOPILOT 60
|
||||
|
||||
// Driver configuration
|
||||
#define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight
|
||||
|
|
|
|||
|
|
@ -709,7 +709,7 @@ void detectAndApplySignalLossBehaviour(void)
|
|||
if (!thisChannelValid) {
|
||||
if (channel == THROTTLE ) {
|
||||
sample = failsafeConfig()->failsafe_throttle;
|
||||
// stage 2 failsafe throttle value. In GPS Rescue Flight mode, gpsRescueGetThrottle overrides, late in mixer.c
|
||||
// stage 2 failsafe throttle value. In GPS Rescue Flight mode, altitude control overrides, late in mixer.c
|
||||
} else {
|
||||
sample = rxConfig()->midrc;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -43,8 +43,10 @@ arming_prevention_unittest_SRC := \
|
|||
$(USER_DIR)/fc/rc_controls.c \
|
||||
$(USER_DIR)/fc/rc_modes.c \
|
||||
$(USER_DIR)/fc/runtime_config.c \
|
||||
$(USER_DIR)/flight/autopilot.c \
|
||||
$(USER_DIR)/flight/gps_rescue.c
|
||||
|
||||
|
||||
arming_prevention_unittest_DEFINES := \
|
||||
USE_GPS_RESCUE=
|
||||
|
||||
|
|
@ -466,6 +468,7 @@ rx_spi_expresslrs_telemetry_unittest_DEFINES := \
|
|||
|
||||
althold_unittest_SRC := \
|
||||
$(USER_DIR)/flight/alt_hold.c \
|
||||
$(USER_DIR)/flight/autopilot.c \
|
||||
$(USER_DIR)/common/maths.c \
|
||||
$(USER_DIR)/pg/rx.c
|
||||
|
||||
|
|
|
|||
|
|
@ -31,11 +31,11 @@ extern "C" {
|
|||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/alt_hold.h"
|
||||
#include "flight/autopilot.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
|
|
@ -43,12 +43,10 @@ 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(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
|
||||
PG_REGISTER(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 0);
|
||||
|
||||
extern altHoldState_t altHoldState;
|
||||
void altHoldReset(void);
|
||||
void altHoldProcessTransitions(void);
|
||||
void altHoldInit(void);
|
||||
void updateAltHoldState(timeUs_t);
|
||||
bool failsafeIsActive(void) { return false; }
|
||||
|
|
@ -100,33 +98,11 @@ 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);
|
||||
}
|
||||
float pt2FilterGain(float, float) {
|
||||
return 0.0f;
|
||||
}
|
||||
float pt2FilterApply(pt2Filter_t *altHoldDeltaLpf, float) {
|
||||
UNUSED(altHoldDeltaLpf);
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
bool isAltitudeAvailable(void) { return true; }
|
||||
float getAltitudeCm(void) {return 0.0f;}
|
||||
float getAltitudeDerivative(void) {return 0.0f;}
|
||||
float getCosTiltAngle(void) { return 0.0f; }
|
||||
float rcCommand[4];
|
||||
|
||||
float getRcDeflection(int)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
void parseRcChannels(const char *input, rxConfig_t *rxConfig)
|
||||
{
|
||||
UNUSED(input);
|
||||
|
|
|
|||
|
|
@ -1,196 +0,0 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
//#define DEBUG_ALTITUDE_HOLD
|
||||
|
||||
#define USE_BARO
|
||||
|
||||
extern "C" {
|
||||
#include "debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
|
||||
#include "io/escservo.h"
|
||||
#include "io/rc_controls.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/position.h"
|
||||
|
||||
#include "config/runtime_config.h"
|
||||
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
#define DOWNWARDS_THRUST true
|
||||
#define UPWARDS_THRUST false
|
||||
|
||||
|
||||
extern "C" {
|
||||
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations);
|
||||
uint16_t calculateTiltAngle(rollAndPitchInclination_t *inclinations);
|
||||
}
|
||||
|
||||
typedef struct inclinationExpectation_s {
|
||||
rollAndPitchInclination_t inclination;
|
||||
bool expectDownwardsThrust;
|
||||
} inclinationExpectation_t;
|
||||
|
||||
TEST(AltitudeHoldTest, IsThrustFacingDownwards)
|
||||
{
|
||||
// given
|
||||
|
||||
inclinationExpectation_t inclinationExpectations[] = {
|
||||
{ {{ 0, 0 }}, DOWNWARDS_THRUST },
|
||||
{ {{ 799, 799 }}, DOWNWARDS_THRUST },
|
||||
{ {{ 800, 799 }}, UPWARDS_THRUST },
|
||||
{ {{ 799, 800 }}, UPWARDS_THRUST },
|
||||
{ {{ 800, 800 }}, UPWARDS_THRUST },
|
||||
{ {{ 801, 801 }}, UPWARDS_THRUST },
|
||||
{ {{ -799, -799 }}, DOWNWARDS_THRUST },
|
||||
{ {{ -800, -799 }}, UPWARDS_THRUST },
|
||||
{ {{ -799, -800 }}, UPWARDS_THRUST },
|
||||
{ {{ -800, -800 }}, UPWARDS_THRUST },
|
||||
{ {{ -801, -801 }}, UPWARDS_THRUST }
|
||||
};
|
||||
uint8_t testIterationCount = sizeof(inclinationExpectations) / sizeof(inclinationExpectation_t);
|
||||
|
||||
// expect
|
||||
|
||||
for (uint8_t index = 0; index < testIterationCount; index ++) {
|
||||
inclinationExpectation_t *angleInclinationExpectation = &inclinationExpectations[index];
|
||||
#ifdef DEBUG_ALTITUDE_HOLD
|
||||
printf("iteration: %d\n", index);
|
||||
#endif
|
||||
bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination);
|
||||
EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result);
|
||||
}
|
||||
}
|
||||
|
||||
typedef struct inclinationAngleExpectations_s {
|
||||
rollAndPitchInclination_t inclination;
|
||||
uint16_t expected_angle;
|
||||
} inclinationAngleExpectations_t;
|
||||
|
||||
TEST(AltitudeHoldTest, TestCalculateTiltAngle)
|
||||
{
|
||||
inclinationAngleExpectations_t inclinationAngleExpectations[] = {
|
||||
{ {{ 0, 0}}, 0},
|
||||
{ {{ 1, 0}}, 1},
|
||||
{ {{ 0, 1}}, 1},
|
||||
{ {{ 0, -1}}, 1},
|
||||
{ {{-1, 0}}, 1},
|
||||
{ {{-1, -2}}, 2},
|
||||
{ {{-2, -1}}, 2},
|
||||
{ {{ 1, 2}}, 2},
|
||||
{ {{ 2, 1}}, 2}
|
||||
};
|
||||
|
||||
rollAndPitchInclination_t inclination = {{0, 0}};
|
||||
uint16_t tilt_angle = calculateTiltAngle(&inclination);
|
||||
EXPECT_EQ(tilt_angle, 0);
|
||||
|
||||
for (uint8_t i = 0; i < 9; i++) {
|
||||
inclinationAngleExpectations_t *expectation = &inclinationAngleExpectations[i];
|
||||
uint16_t result = calculateTiltAngle(&expectation->inclination);
|
||||
EXPECT_EQ(expectation->expected_angle, result);
|
||||
}
|
||||
}
|
||||
|
||||
// STUBS
|
||||
|
||||
extern "C" {
|
||||
uint32_t rcModeActivationMask;
|
||||
float rcCommand[4];
|
||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
|
||||
uint32_t accTimeSum ; // keep track for integration of acc
|
||||
int accSumCount;
|
||||
float accVelScale;
|
||||
|
||||
rollAndPitchInclination_t inclination;
|
||||
|
||||
//uint16_t acc_1G;
|
||||
//int16_t heading;
|
||||
//gyro_t gyro;
|
||||
int32_t accSum[XYZ_AXIS_COUNT];
|
||||
//int16_t magADC[XYZ_AXIS_COUNT];
|
||||
int32_t BaroAlt;
|
||||
int16_t debug[DEBUG16_VALUE_COUNT];
|
||||
|
||||
uint8_t stateFlags;
|
||||
uint16_t flightModeFlags;
|
||||
uint8_t armingFlags;
|
||||
|
||||
int32_t sonarAlt;
|
||||
|
||||
|
||||
uint16_t enableFlightMode(flightModeFlags_e mask)
|
||||
{
|
||||
return flightModeFlags |= (mask);
|
||||
}
|
||||
|
||||
uint16_t disableFlightMode(flightModeFlags_e mask)
|
||||
{
|
||||
return flightModeFlags &= ~(mask);
|
||||
}
|
||||
|
||||
void gyroUpdate(void) {};
|
||||
bool sensors(uint32_t mask)
|
||||
{
|
||||
UNUSED(mask);
|
||||
return false;
|
||||
};
|
||||
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
|
||||
{
|
||||
UNUSED(rollAndPitchTrims);
|
||||
}
|
||||
|
||||
void imuResetAccelerationSum(void) {};
|
||||
|
||||
int32_t applyDeadband(int32_t, int32_t) { return 0; }
|
||||
uint32_t micros(void) { return 0; }
|
||||
bool isBaroCalibrationComplete(void) { return true; }
|
||||
void performBaroCalibrationCycle(void) {}
|
||||
int32_t baroCalculateAltitude(void) { return 0; }
|
||||
int constrain(int amt, int low, int high)
|
||||
{
|
||||
UNUSED(amt);
|
||||
UNUSED(low);
|
||||
UNUSED(high);
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -36,13 +36,13 @@ extern "C" {
|
|||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/autopilot.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/gps_rescue.h"
|
||||
#include "flight/imu.h"
|
||||
#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"
|
||||
|
|
@ -78,7 +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);
|
||||
PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
|
||||
|
||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
uint16_t averageSystemLoadPercent = 0;
|
||||
|
|
@ -1137,10 +1137,6 @@ extern "C" {
|
|||
void pinioBoxTaskControl(void) {}
|
||||
void schedulerSetNextStateTime(timeDelta_t) {}
|
||||
|
||||
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;}
|
||||
|
||||
|
|
@ -1164,5 +1160,4 @@ extern "C" {
|
|||
void getRcDeflectionAbs(void) {}
|
||||
uint32_t getCpuPercentageLate(void) { return 0; }
|
||||
bool crashFlipSuccessful(void) { return false; }
|
||||
bool isBelowLandingAltitude(void) { return false; };
|
||||
}
|
||||
|
|
|
|||
|
|
@ -41,12 +41,12 @@ extern "C" {
|
|||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "fc/rc.h"
|
||||
|
||||
|
||||
#include "flight/autopilot.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/position_control.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
||||
|
|
@ -74,7 +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_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
|
||||
|
||||
PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
|
||||
.enabledFeatures = 0
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue