1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00

Replaced config pointers with config PG macros. Tidied #includes

This commit is contained in:
Martin Budden 2017-01-15 08:27:02 +00:00
parent b83d1e4176
commit 7529b45aa4
15 changed files with 84 additions and 132 deletions

View file

@ -58,6 +58,8 @@
#include "config/config_master.h" #include "config/config_master.h"
#include "config/feature.h" #include "config/feature.h"
#include "rx/rx.h"
// For VISIBLE* (Actually, included by config_master.h) // For VISIBLE* (Actually, included by config_master.h)
#include "io/osd.h" #include "io/osd.h"

View file

@ -208,7 +208,7 @@ static long cmsx_menuPidAltMag_onExit(const OSD_Entry *self)
cmsx_WritebackPidFromArray(cmsx_pidVel, PIDVEL); cmsx_WritebackPidFromArray(cmsx_pidVel, PIDVEL);
pidProfileMutable()->P8[PIDMAG] = cmsx_pidMag[0]; pidProfileMutable()->P8[PIDMAG] = cmsx_pidMag[0];
navigationUsePIDs(pidProfile()); navigationUsePIDs();
return 0; return 0;
} }
@ -258,7 +258,7 @@ static long cmsx_menuPidGpsnav_onExit(const OSD_Entry *self)
cmsx_WritebackPidFromArray(cmsx_pidPosR, PIDPOSR); cmsx_WritebackPidFromArray(cmsx_pidPosR, PIDPOSR);
cmsx_WritebackPidFromArray(cmsx_pidNavR, PIDNAVR); cmsx_WritebackPidFromArray(cmsx_pidNavR, PIDNAVR);
navigationUsePIDs(pidProfile()); navigationUsePIDs();
return 0; return 0;
} }

View file

@ -44,6 +44,8 @@
#include "cms/cms_types.h" #include "cms/cms_types.h"
#include "cms/cms_menu_ledstrip.h" #include "cms/cms_menu_ledstrip.h"
#include "rx/rx.h"
// //
// Misc // Misc
// //

View file

@ -393,11 +393,7 @@ static void activateConfig(void)
pidInit(); pidInit();
#ifdef NAV #ifdef NAV
navigationUsePIDs(pidProfile()); navigationUsePIDs();
navigationUseRcControlsConfig(rcControlsConfig());
navigationUseRxConfig(rxConfig());
navigationUseFlight3DConfig(flight3DConfig());
navigationUsemotorConfig(motorConfig());
#endif #endif
} }

View file

@ -527,13 +527,7 @@ void init(void)
#endif #endif
#ifdef NAV #ifdef NAV
navigationInit( navigationInit();
&currentProfile->pidProfile,
rcControlsConfig(),
rxConfig(),
flight3DConfig(),
motorConfig()
);
#endif #endif
#ifdef LED_STRIP #ifdef LED_STRIP

View file

@ -1300,7 +1300,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
} }
schedulePidGainsUpdate(); schedulePidGainsUpdate();
#if defined(NAV) #if defined(NAV)
navigationUsePIDs(&currentProfile->pidProfile); navigationUsePIDs();
#endif #endif
break; break;

View file

@ -47,6 +47,7 @@
#include "flight/navigation_rewrite_private.h" #include "flight/navigation_rewrite_private.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
/*----------------------------------------------------------- /*-----------------------------------------------------------
@ -2536,51 +2537,29 @@ void updateWaypointsAndNavigationMode(void)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* NAV main control functions * NAV main control functions
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void navigationUseRcControlsConfig(const rcControlsConfig_t *initialRcControlsConfig) void navigationUsePIDs(void)
{ {
posControl.rcControlsConfig = initialRcControlsConfig;
}
void navigationUseFlight3DConfig(const flight3DConfig_t * initialFlight3DConfig)
{
posControl.flight3DConfig = initialFlight3DConfig;
}
void navigationUseRxConfig(const rxConfig_t * initialRxConfig)
{
posControl.rxConfig = initialRxConfig;
}
void navigationUsemotorConfig(const motorConfig_t * initialmotorConfig)
{
posControl.motorConfig = initialmotorConfig;
}
void navigationUsePIDs(const pidProfile_t *initialPidProfile)
{
posControl.pidProfile = initialPidProfile;
// Brake time parameter // Brake time parameter
posControl.posDecelerationTime = (float)posControl.pidProfile->I8[PIDPOS] / 100.0f; posControl.posDecelerationTime = (float)pidProfile()->I8[PIDPOS] / 100.0f;
// Position controller expo (taret vel expo for MC) // Position controller expo (taret vel expo for MC)
posControl.posResponseExpo = constrainf((float)posControl.pidProfile->D8[PIDPOS] / 100.0f, 0.0f, 1.0f); posControl.posResponseExpo = constrainf((float)pidProfile()->D8[PIDPOS] / 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++) {
navPInit(&posControl.pids.pos[axis], (float)posControl.pidProfile->P8[PIDPOS] / 100.0f); navPInit(&posControl.pids.pos[axis], (float)pidProfile()->P8[PIDPOS] / 100.0f);
navPidInit(&posControl.pids.vel[axis], (float)posControl.pidProfile->P8[PIDPOSR] / 100.0f, navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->P8[PIDPOSR] / 100.0f,
(float)posControl.pidProfile->I8[PIDPOSR] / 100.0f, (float)pidProfile()->I8[PIDPOSR] / 100.0f,
(float)posControl.pidProfile->D8[PIDPOSR] / 100.0f); (float)pidProfile()->D8[PIDPOSR] / 100.0f);
} }
// Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
navPInit(&posControl.pids.pos[Z], (float)posControl.pidProfile->P8[PIDALT] / 100.0f); navPInit(&posControl.pids.pos[Z], (float)pidProfile()->P8[PIDALT] / 100.0f);
navPidInit(&posControl.pids.vel[Z], (float)posControl.pidProfile->P8[PIDVEL] / 100.0f, navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->P8[PIDVEL] / 100.0f,
(float)posControl.pidProfile->I8[PIDVEL] / 100.0f, (float)pidProfile()->I8[PIDVEL] / 100.0f,
(float)posControl.pidProfile->D8[PIDVEL] / 100.0f); (float)pidProfile()->D8[PIDVEL] / 100.0f);
// Initialize surface tracking PID // Initialize surface tracking PID
navPidInit(&posControl.pids.surface, 2.0f, navPidInit(&posControl.pids.surface, 2.0f,
@ -2588,21 +2567,16 @@ void navigationUsePIDs(const pidProfile_t *initialPidProfile)
0.0f); 0.0f);
// Initialize fixed wing PID controllers // Initialize fixed wing PID controllers
navPidInit(&posControl.pids.fw_nav, (float)posControl.pidProfile->P8[PIDNAVR] / 100.0f, navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->P8[PIDNAVR] / 100.0f,
(float)posControl.pidProfile->I8[PIDNAVR] / 100.0f, (float)pidProfile()->I8[PIDNAVR] / 100.0f,
(float)posControl.pidProfile->D8[PIDNAVR] / 100.0f); (float)pidProfile()->D8[PIDNAVR] / 100.0f);
navPidInit(&posControl.pids.fw_alt, (float)posControl.pidProfile->P8[PIDALT] / 100.0f, navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->P8[PIDALT] / 100.0f,
(float)posControl.pidProfile->I8[PIDALT] / 100.0f, (float)pidProfile()->I8[PIDALT] / 100.0f,
(float)posControl.pidProfile->D8[PIDALT] / 100.0f); (float)pidProfile()->D8[PIDALT] / 100.0f);
} }
void navigationInit( void navigationInit(void)
const pidProfile_t *initialPidProfile,
const rcControlsConfig_t *initialRcControlsConfig,
const rxConfig_t * initialRxConfig,
const flight3DConfig_t * initialFlight3DConfig,
const motorConfig_t * initialmotorConfig)
{ {
/* Initial state */ /* Initial state */
posControl.navState = NAV_STATE_IDLE; posControl.navState = NAV_STATE_IDLE;
@ -2628,11 +2602,7 @@ void navigationInit(
posControl.actualState.surfaceMin = -1.0f; posControl.actualState.surfaceMin = -1.0f;
/* Use system config */ /* Use system config */
navigationUsePIDs(initialPidProfile); navigationUsePIDs();
navigationUseRcControlsConfig(initialRcControlsConfig);
navigationUseRxConfig(initialRxConfig);
navigationUsemotorConfig(initialmotorConfig);
navigationUseFlight3DConfig(initialFlight3DConfig);
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------

View file

@ -18,16 +18,10 @@
#pragma once #pragma once
#include "common/maths.h" #include "common/maths.h"
#include "common/filter.h"
#include "fc/rc_controls.h"
#include "io/gps.h"
#include "flight/pid.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/mixer.h"
#include "rx/rx.h" #include "io/gps.h"
/* GPS Home location data */ /* GPS Home location data */
extern gpsLocation_t GPS_home; extern gpsLocation_t GPS_home;
@ -235,18 +229,8 @@ typedef struct {
navWaypointActions_e activeWpAction; navWaypointActions_e activeWpAction;
} navSystemStatus_t; } navSystemStatus_t;
void navigationUsePIDs(const pidProfile_t *pidProfile); void navigationUsePIDs(void);
void navigationUseRcControlsConfig(const rcControlsConfig_t *initialRcControlsConfig); void navigationInit(void);
void navigationUseRxConfig(const rxConfig_t * initialRxConfig);
struct motorConfig_s;
void navigationUsemotorConfig(const struct motorConfig_s * initialmotorConfig);
void navigationUseFlight3DConfig(const flight3DConfig_t * initialFlight3DConfig);
void navigationInit(
const pidProfile_t *initialPidProfile,
const rcControlsConfig_t *initialRcControlsConfig,
const rxConfig_t * initialRxConfig,
const flight3DConfig_t * initialFlight3DConfig,
const struct motorConfig_s * initialmotorConfig);
/* Navigation system updates */ /* Navigation system updates */
void updateWaypointsAndNavigationMode(void); void updateWaypointsAndNavigationMode(void);

View file

@ -38,11 +38,13 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation_rewrite.h" #include "flight/navigation_rewrite.h"
#include "flight/navigation_rewrite_private.h" #include "flight/navigation_rewrite_private.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/controlrate_profile.h" #include "fc/controlrate_profile.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
// If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind // If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind
@ -75,11 +77,11 @@ void resetFixedWingAltitudeController()
bool adjustFixedWingAltitudeFromRCInput(void) bool adjustFixedWingAltitudeFromRCInput(void)
{ {
int16_t rcAdjustment = applyDeadband(rcCommand[PITCH], posControl.rcControlsConfig->alt_hold_deadband); int16_t rcAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband);
if (rcAdjustment) { if (rcAdjustment) {
// set velocity proportional to stick movement // set velocity proportional to stick movement
float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - posControl.rcControlsConfig->alt_hold_deadband); float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband);
updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_RESET_SURFACE_TARGET); updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_RESET_SURFACE_TARGET);
return true; return true;
} }
@ -223,7 +225,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// Shift position according to pilot's ROLL input (up to max_manual_speed velocity) // Shift position according to pilot's ROLL input (up to max_manual_speed velocity)
if (posControl.flags.isAdjustingPosition) { if (posControl.flags.isAdjustingPosition) {
int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], posControl.rcControlsConfig->pos_hold_deadband); int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
if (rcRollAdjustment) { if (rcRollAdjustment) {
float rcShiftY = rcRollAdjustment * navConfig()->general.max_manual_speed / 500.0f * trackingPeriod; float rcShiftY = rcRollAdjustment * navConfig()->general.max_manual_speed / 500.0f * trackingPeriod;
@ -239,7 +241,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
bool adjustFixedWingPositionFromRCInput(void) bool adjustFixedWingPositionFromRCInput(void)
{ {
int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], posControl.rcControlsConfig->pos_hold_deadband); int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
return (rcRollAdjustment); return (rcRollAdjustment);
} }
@ -415,12 +417,12 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) { if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
// PITCH angle is measured in opposite direction ( >0 - dive, <0 - climb) // PITCH angle is measured in opposite direction ( >0 - dive, <0 - climb)
pitchCorrection = constrain(pitchCorrection, -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle)); pitchCorrection = constrain(pitchCorrection, -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, posControl.pidProfile->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
} }
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) { if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
rollCorrection = constrain(rollCorrection, -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle)); rollCorrection = constrain(rollCorrection, -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, posControl.pidProfile->max_angle_inclination[FD_ROLL]); rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
// Calculate coordinated turn rate based on velocity and banking angle // Calculate coordinated turn rate based on velocity and banking angle
if (posControl.actualState.velXY >= 300.0f) { if (posControl.actualState.velXY >= 300.0f) {
@ -434,7 +436,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) { if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
rcCommand[THROTTLE] = constrain(correctedThrottleValue, posControl.motorConfig->minthrottle, posControl.motorConfig->maxthrottle); rcCommand[THROTTLE] = constrain(correctedThrottleValue, motorConfig()->minthrottle, motorConfig()->maxthrottle);
} }
} }
@ -465,8 +467,8 @@ bool isFixedWingLandingDetected(void)
void applyFixedWingEmergencyLandingController(void) void applyFixedWingEmergencyLandingController(void)
{ {
// FIXME: Make this configurable, use altitude controller if available (similar to MC code) // FIXME: Make this configurable, use altitude controller if available (similar to MC code)
rcCommand[PITCH] = pidAngleToRcCommand(-FW_EMERGENCY_DIVE_DECIDEG, posControl.pidProfile->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(-FW_EMERGENCY_DIVE_DECIDEG, pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[ROLL] = pidAngleToRcCommand(-FW_EMERGENCY_ROLL_DECIDEG, posControl.pidProfile->max_angle_inclination[FD_ROLL]); rcCommand[ROLL] = pidAngleToRcCommand(-FW_EMERGENCY_ROLL_DECIDEG, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[YAW] = pidRateToRcCommand(-FW_EMERGENCY_YAW_RATE_DPS, currentControlRateProfile->rates[FD_YAW]); rcCommand[YAW] = pidRateToRcCommand(-FW_EMERGENCY_YAW_RATE_DPS, currentControlRateProfile->rates[FD_YAW]);
rcCommand[THROTTLE] = navConfig()->fw.min_throttle; rcCommand[THROTTLE] = navConfig()->fw.min_throttle;
} }

View file

@ -46,10 +46,12 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation_rewrite.h" #include "flight/navigation_rewrite.h"
#include "flight/navigation_rewrite_private.h" #include "flight/navigation_rewrite_private.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "config/feature.h" #include "config/feature.h"
@ -128,7 +130,7 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs)
const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs- launchState.launchStartedTime); const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs- launchState.launchStartedTime);
// If user moves the stick - finish the launch // If user moves the stick - finish the launch
if ((ABS(rcCommand[ROLL]) > posControl.rcControlsConfig->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > posControl.rcControlsConfig->pos_hold_deadband)) { if ((ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband)) {
launchState.launchFinished = true; launchState.launchFinished = true;
} }
@ -149,7 +151,7 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs)
// Throttle control logic // Throttle control logic
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
rcCommand[THROTTLE] = posControl.motorConfig->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle rcCommand[THROTTLE] = motorConfig()->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle
} }
} }
} }
@ -162,7 +164,7 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs)
// Throttle control logic // Throttle control logic
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
rcCommand[THROTTLE] = posControl.motorConfig->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle rcCommand[THROTTLE] = motorConfig()->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle
} }
// Control beeper // Control beeper
@ -172,7 +174,7 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs)
// Lock out controls // Lock out controls
rcCommand[ROLL] = 0; rcCommand[ROLL] = 0;
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), posControl.pidProfile->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = 0; rcCommand[YAW] = 0;
} }

View file

@ -27,6 +27,7 @@
#include "build/debug.h" #include "build/debug.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h" #include "common/maths.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"

View file

@ -46,6 +46,7 @@
#include "flight/navigation_rewrite.h" #include "flight/navigation_rewrite.h"
#include "flight/navigation_rewrite_private.h" #include "flight/navigation_rewrite_private.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/mixer.h"
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Altitude controller for multicopter aircraft * Altitude controller for multicopter aircraft
@ -104,8 +105,8 @@ static void updateAltitudeVelocityController_MC(uint32_t deltaMicros)
static void updateAltitudeThrottleController_MC(uint32_t deltaMicros) static void updateAltitudeThrottleController_MC(uint32_t deltaMicros)
{ {
// Calculate min and max throttle boundaries (to compensate for integral windup) // Calculate min and max throttle boundaries (to compensate for integral windup)
const int16_t thrAdjustmentMin = (int16_t)posControl.motorConfig->minthrottle - (int16_t)navConfig()->mc.hover_throttle; const int16_t thrAdjustmentMin = (int16_t)motorConfig()->minthrottle - (int16_t)navConfig()->mc.hover_throttle;
const int16_t thrAdjustmentMax = (int16_t)posControl.motorConfig->maxthrottle - (int16_t)navConfig()->mc.hover_throttle; const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle;
posControl.rcAdjustment[THROTTLE] = navPidApply2(posControl.desiredState.vel.V.Z, posControl.actualState.vel.V.Z, US2S(deltaMicros), &posControl.pids.vel[Z], thrAdjustmentMin, thrAdjustmentMax, false); posControl.rcAdjustment[THROTTLE] = navPidApply2(posControl.desiredState.vel.V.Z, posControl.actualState.vel.V.Z, US2S(deltaMicros), &posControl.pids.vel[Z], thrAdjustmentMin, thrAdjustmentMax, false);
@ -116,18 +117,18 @@ static void updateAltitudeThrottleController_MC(uint32_t deltaMicros)
bool adjustMulticopterAltitudeFromRCInput(void) bool adjustMulticopterAltitudeFromRCInput(void)
{ {
int16_t rcThrottleAdjustment = rcCommand[THROTTLE] - altHoldThrottleRCZero; int16_t rcThrottleAdjustment = rcCommand[THROTTLE] - altHoldThrottleRCZero;
if (ABS(rcThrottleAdjustment) > posControl.rcControlsConfig->alt_hold_deadband) { if (ABS(rcThrottleAdjustment) > rcControlsConfig()->alt_hold_deadband) {
// set velocity proportional to stick movement // set velocity proportional to stick movement
float rcClimbRate; float rcClimbRate;
// Make sure we can satisfy max_manual_climb_rate in both up and down directions // Make sure we can satisfy max_manual_climb_rate in both up and down directions
if (rcThrottleAdjustment > 0) { if (rcThrottleAdjustment > 0) {
// Scaling from altHoldThrottleRCZero to maxthrottle // Scaling from altHoldThrottleRCZero to maxthrottle
rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (posControl.motorConfig->maxthrottle - altHoldThrottleRCZero); rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (motorConfig()->maxthrottle - altHoldThrottleRCZero);
} }
else { else {
// Scaling from minthrottle to altHoldThrottleRCZero // Scaling from minthrottle to altHoldThrottleRCZero
rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (altHoldThrottleRCZero - posControl.motorConfig->minthrottle); rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (altHoldThrottleRCZero - motorConfig()->minthrottle);
} }
updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_UPDATE_SURFACE_TARGET); updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_UPDATE_SURFACE_TARGET);
@ -146,7 +147,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
void setupMulticopterAltitudeController(void) void setupMulticopterAltitudeController(void)
{ {
const throttleStatus_e throttleStatus = calculateThrottleStatus(posControl.flight3DConfig->deadband3d_throttle); const throttleStatus_e throttleStatus = calculateThrottleStatus(flight3DConfig()->deadband3d_throttle);
if (navConfig()->general.flags.use_thr_mid_for_althold) { if (navConfig()->general.flags.use_thr_mid_for_althold) {
altHoldThrottleRCZero = rcLookupThrottleMid(); altHoldThrottleRCZero = rcLookupThrottleMid();
@ -163,8 +164,8 @@ void setupMulticopterAltitudeController(void)
// Make sure we are able to satisfy the deadband // Make sure we are able to satisfy the deadband
altHoldThrottleRCZero = constrain(altHoldThrottleRCZero, altHoldThrottleRCZero = constrain(altHoldThrottleRCZero,
posControl.motorConfig->minthrottle + posControl.rcControlsConfig->alt_hold_deadband + 10, motorConfig()->minthrottle + rcControlsConfig()->alt_hold_deadband + 10,
posControl.motorConfig->maxthrottle - posControl.rcControlsConfig->alt_hold_deadband - 10); motorConfig()->maxthrottle - rcControlsConfig()->alt_hold_deadband - 10);
/* Force AH controller to initialize althold integral for pending takeoff on reset */ /* Force AH controller to initialize althold integral for pending takeoff on reset */
if (throttleStatus == THROTTLE_LOW) { if (throttleStatus == THROTTLE_LOW) {
@ -224,7 +225,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
} }
// Update throttle controller // Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.motorConfig->minthrottle, posControl.motorConfig->maxthrottle); rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle);
// Save processed throttle for future use // Save processed throttle for future use
rcCommandAdjustedThrottle = rcCommand[THROTTLE]; rcCommandAdjustedThrottle = rcCommand[THROTTLE];
@ -235,7 +236,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
bool adjustMulticopterHeadingFromRCInput(void) bool adjustMulticopterHeadingFromRCInput(void)
{ {
if (ABS(rcCommand[YAW]) > posControl.rcControlsConfig->pos_hold_deadband) { if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) {
// Can only allow pilot to set the new heading if doing PH, during RTH copter will target itself to home // Can only allow pilot to set the new heading if doing PH, during RTH copter will target itself to home
posControl.desiredState.yaw = posControl.actualState.yaw; posControl.desiredState.yaw = posControl.actualState.yaw;
@ -266,8 +267,8 @@ void resetMulticopterPositionController(void)
bool adjustMulticopterPositionFromRCInput(void) bool adjustMulticopterPositionFromRCInput(void)
{ {
const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], posControl.rcControlsConfig->pos_hold_deadband); const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband);
const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], posControl.rcControlsConfig->pos_hold_deadband); const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
if (rcPitchAdjustment || rcRollAdjustment) { if (rcPitchAdjustment || rcRollAdjustment) {
// If mode is GPS_CRUISE, move target position, otherwise POS controller will passthru the RC input to ANGLE PID // If mode is GPS_CRUISE, move target position, otherwise POS controller will passthru the RC input to ANGLE PID
@ -461,8 +462,8 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
} }
if (!bypassPositionController) { if (!bypassPositionController) {
rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], posControl.pidProfile->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], posControl.pidProfile->max_angle_inclination[FD_ROLL]); rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
} }
} }
@ -579,7 +580,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
} }
// Update throttle controller // Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.motorConfig->minthrottle, posControl.motorConfig->maxthrottle); rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle);
} }
else { else {
/* Sensors has gone haywire, attempt to land regardless */ /* Sensors has gone haywire, attempt to land regardless */
@ -587,7 +588,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
} }
else { else {
rcCommand[THROTTLE] = posControl.motorConfig->minthrottle; rcCommand[THROTTLE] = motorConfig()->minthrottle;
} }
} }
} }

View file

@ -35,23 +35,25 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "sensors/sensors.h" #include "fc/config.h"
#include "sensors/rangefinder.h" #include "fc/rc_controls.h"
#include "sensors/barometer.h" #include "fc/runtime_config.h"
#include "sensors/pitotmeter.h"
#include "sensors/acceleration.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "io/gps.h"
#include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/navigation_rewrite.h" #include "flight/navigation_rewrite.h"
#include "flight/navigation_rewrite_private.h" #include "flight/navigation_rewrite_private.h"
#include "flight/pid.h"
#include "io/gps.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/pitotmeter.h"
#include "sensors/rangefinder.h"
#include "sensors/sensors.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
/** /**
* Model-identification based position estimator * Model-identification based position estimator

View file

@ -21,8 +21,8 @@
#if defined(NAV) #if defined(NAV)
#include "common/filter.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "rx/rx.h"
#define MIN_POSITION_UPDATE_RATE_HZ 5 // Minimum position update rate at which XYZ controllers would be applied #define MIN_POSITION_UPDATE_RATE_HZ 5 // Minimum position update rate at which XYZ controllers would be applied
#define NAV_THROTTLE_CUTOFF_FREQENCY_HZ 4 // low-pass filter on throttle output #define NAV_THROTTLE_CUTOFF_FREQENCY_HZ 4 // low-pass filter on throttle output
@ -246,7 +246,6 @@ typedef struct {
navigationFSMState_t onEvent[NAV_FSM_EVENT_COUNT]; navigationFSMState_t onEvent[NAV_FSM_EVENT_COUNT];
} navigationFSMStateDescriptor_t; } navigationFSMStateDescriptor_t;
struct motorConfig_s;
typedef struct { typedef struct {
/* Flags and navigation system state */ /* Flags and navigation system state */
navigationFSMState_t navState; navigationFSMState_t navState;
@ -286,11 +285,6 @@ typedef struct {
/* Internals */ /* Internals */
int16_t rcAdjustment[4]; int16_t rcAdjustment[4];
const rcControlsConfig_t * rcControlsConfig;
const pidProfile_t * pidProfile;
const rxConfig_t * rxConfig;
const flight3DConfig_t * flight3DConfig;
const struct motorConfig_s * motorConfig;
} navigationPosControl_t; } navigationPosControl_t;
extern navigationPosControl_t posControl; extern navigationPosControl_t posControl;

View file

@ -54,6 +54,8 @@
#include "io/gps.h" #include "io/gps.h"
#endif #endif
#include "rx/rx.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/compass.h" #include "sensors/compass.h"