mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
Remove PosHold support settings from PID bank
This commit is contained in:
parent
ad0d14e2a5
commit
15cdc109f9
5 changed files with 21 additions and 15 deletions
|
@ -437,5 +437,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag
|
||||||
| nav_mc_braking_boost_speed_threshold | 150 | BOOST can be enabled when speed is above this value |
|
| nav_mc_braking_boost_speed_threshold | 150 | BOOST can be enabled when speed is above this value |
|
||||||
| nav_mc_braking_boost_disengage_speed | 100 | BOOST will be disabled when speed goes below this value |
|
| nav_mc_braking_boost_disengage_speed | 100 | BOOST will be disabled when speed goes below this value |
|
||||||
| nav_mc_braking_bank_angle | 40 | max angle that MR is allowed to bank in BOOST mode |
|
| nav_mc_braking_bank_angle | 40 | max angle that MR is allowed to bank in BOOST mode |
|
||||||
|
| nav_mc_pos_deceleration_time | 120 | Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting |
|
||||||
|
| nav_mc_pos_expo | 10 | Expo for PosHold control |
|
||||||
| osd_artificial_horizon_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon |
|
| osd_artificial_horizon_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon |
|
||||||
| baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. |
|
| baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. |
|
||||||
|
|
|
@ -1048,16 +1048,6 @@ groups:
|
||||||
condition: USE_NAV
|
condition: USE_NAV
|
||||||
min: 0
|
min: 0
|
||||||
max: 255
|
max: 255
|
||||||
- name: nav_mc_pos_xy_i
|
|
||||||
field: bank_mc.pid[PID_POS_XY].I
|
|
||||||
condition: USE_NAV
|
|
||||||
min: 0
|
|
||||||
max: 255
|
|
||||||
- name: nav_mc_pos_xy_d
|
|
||||||
field: bank_mc.pid[PID_POS_XY].D
|
|
||||||
condition: USE_NAV
|
|
||||||
min: 0
|
|
||||||
max: 255
|
|
||||||
- name: nav_mc_vel_xy_p
|
- name: nav_mc_vel_xy_p
|
||||||
field: bank_mc.pid[PID_VEL_XY].P
|
field: bank_mc.pid[PID_VEL_XY].P
|
||||||
condition: USE_NAV
|
condition: USE_NAV
|
||||||
|
@ -1376,6 +1366,16 @@ groups:
|
||||||
condition: USE_MR_BRAKING_MODE
|
condition: USE_MR_BRAKING_MODE
|
||||||
min: 15
|
min: 15
|
||||||
max: 60
|
max: 60
|
||||||
|
- name: nav_mc_pos_deceleration_time
|
||||||
|
field: mc.posDecelerationTime
|
||||||
|
condition: USE_NAV
|
||||||
|
min: 0
|
||||||
|
max: 255
|
||||||
|
- name: nav_mc_pos_expo
|
||||||
|
field: mc.posResponseExpo
|
||||||
|
condition: USE_NAV
|
||||||
|
min: 0
|
||||||
|
max: 255
|
||||||
- name: nav_fw_cruise_thr
|
- name: nav_fw_cruise_thr
|
||||||
field: fw.cruise_throttle
|
field: fw.cruise_throttle
|
||||||
min: 1000
|
min: 1000
|
||||||
|
|
|
@ -126,8 +126,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
|
||||||
[PID_HEADING] = { 60, 0, 0, 0 },
|
[PID_HEADING] = { 60, 0, 0, 0 },
|
||||||
[PID_POS_XY] = {
|
[PID_POS_XY] = {
|
||||||
.P = 65, // NAV_POS_XY_P * 100
|
.P = 65, // NAV_POS_XY_P * 100
|
||||||
.I = 120, // posDecelerationTime * 100
|
.I = 0,
|
||||||
.D = 10, // posResponseExpo * 100
|
.D = 0,
|
||||||
.FF = 0,
|
.FF = 0,
|
||||||
},
|
},
|
||||||
[PID_VEL_XY] = {
|
[PID_VEL_XY] = {
|
||||||
|
|
|
@ -77,7 +77,7 @@ PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList);
|
||||||
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
|
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3);
|
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4);
|
||||||
|
|
||||||
PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
||||||
.general = {
|
.general = {
|
||||||
|
@ -127,6 +127,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
||||||
.braking_boost_speed_threshold = 150, // Boost can happen only above 1.5m/s
|
.braking_boost_speed_threshold = 150, // Boost can happen only above 1.5m/s
|
||||||
.braking_boost_disengage_speed = 100, // Disable boost at 1m/s
|
.braking_boost_disengage_speed = 100, // Disable boost at 1m/s
|
||||||
.braking_bank_angle = 40, // Max braking angle
|
.braking_bank_angle = 40, // Max braking angle
|
||||||
|
.posDecelerationTime = 120, // posDecelerationTime * 100
|
||||||
|
.posResponseExpo = 10, // posResponseExpo * 100
|
||||||
},
|
},
|
||||||
|
|
||||||
// Fixed wing
|
// Fixed wing
|
||||||
|
@ -3025,10 +3027,10 @@ void navigationUsePIDs(void)
|
||||||
{
|
{
|
||||||
/** Multicopter PIDs */
|
/** Multicopter PIDs */
|
||||||
// Brake time parameter
|
// Brake time parameter
|
||||||
posControl.posDecelerationTime = (float)pidProfile()->bank_mc.pid[PID_POS_XY].I / 100.0f;
|
posControl.posDecelerationTime = (float)navConfig()->mc.posDecelerationTime / 100.0f;
|
||||||
|
|
||||||
// Position controller expo (taret vel expo for MC)
|
// Position controller expo (taret vel expo for MC)
|
||||||
posControl.posResponseExpo = constrainf((float)pidProfile()->bank_mc.pid[PID_POS_XY].D / 100.0f, 0.0f, 1.0f);
|
posControl.posResponseExpo = constrainf((float)navConfig()->mc.posResponseExpo / 100.0f, 0.0f, 1.0f);
|
||||||
|
|
||||||
// Initialize position hold P-controller
|
// Initialize position hold P-controller
|
||||||
for (int axis = 0; axis < 2; axis++) {
|
for (int axis = 0; axis < 2; axis++) {
|
||||||
|
|
|
@ -167,6 +167,8 @@ typedef struct navConfig_s {
|
||||||
uint16_t braking_boost_speed_threshold; // Above this speed braking boost mode can engage
|
uint16_t braking_boost_speed_threshold; // Above this speed braking boost mode can engage
|
||||||
uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage
|
uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage
|
||||||
uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase
|
uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase
|
||||||
|
uint8_t posDecelerationTime; // Brake time parameter
|
||||||
|
uint8_t posResponseExpo; // Position controller expo (taret vel expo for MC)
|
||||||
} mc;
|
} mc;
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue