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:
parent
b83d1e4176
commit
7529b45aa4
15 changed files with 84 additions and 132 deletions
|
@ -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"
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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
|
||||||
//
|
//
|
||||||
|
|
|
@ -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
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -527,13 +527,7 @@ void init(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef NAV
|
#ifdef NAV
|
||||||
navigationInit(
|
navigationInit();
|
||||||
¤tProfile->pidProfile,
|
|
||||||
rcControlsConfig(),
|
|
||||||
rxConfig(),
|
|
||||||
flight3DConfig(),
|
|
||||||
motorConfig()
|
|
||||||
);
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef LED_STRIP
|
#ifdef LED_STRIP
|
||||||
|
|
|
@ -1300,7 +1300,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
||||||
}
|
}
|
||||||
schedulePidGainsUpdate();
|
schedulePidGainsUpdate();
|
||||||
#if defined(NAV)
|
#if defined(NAV)
|
||||||
navigationUsePIDs(¤tProfile->pidProfile);
|
navigationUsePIDs();
|
||||||
#endif
|
#endif
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
|
|
@ -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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*-----------------------------------------------------------
|
/*-----------------------------------------------------------
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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"
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue