Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

Merge pull request #117 from iNavFlight/thr-mid-hover

Browse source 
NAV: Make altitude hold aware of throttle expo curve midpoint. Closes #105
This commit is contained in:
Konstantin Sharlaimov 2016-03-27 21:15:36 +10:00
commit 3c5ea84863
10 changed files with 57 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/config/config.c
View file

@ -216,6 +216,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
void resetNavConfig(navConfig_t * navConfig)
{
// Navigation flags
navConfig->flags.use_thr_mid_for_althold = 1;
navConfig->flags.extra_arming_safety = 1;
navConfig->flags.user_control_mode = NAV_GPS_ATTI;
navConfig->flags.rth_alt_control_style = NAV_RTH_AT_LEAST_ALT;
@ -802,6 +803,7 @@ void activateConfig(void)
navigationUsePIDs(&currentProfile->pidProfile);
navigationUseRcControlsConfig(&currentProfile->rcControlsConfig);
navigationUseRxConfig(&masterConfig.rxConfig);
navigationUseFlight3DConfig(&masterConfig.flight3DConfig);
navigationUseEscAndServoConfig(&masterConfig.escAndServoConfig);
#endif

1
src/main/drivers/pwm_rx.c
View file

@ -252,7 +252,6 @@ static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture
ppmDev.numChannels = ppmDev.pulseIndex;
}
} else {
debug[2]++;
ppmDev.stableFramesSeenCount = 0;
}

7
src/main/flight/navigation_rewrite.c
View file

@ -2118,6 +2118,11 @@ void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig)
posControl.rcControlsConfig = initialRcControlsConfig;
}
void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig)
{
posControl.flight3DConfig = initialFlight3DConfig;
}
void navigationUseRxConfig(rxConfig_t * initialRxConfig)
{
posControl.rxConfig = initialRxConfig;
@ -2170,6 +2175,7 @@ void navigationInit(navConfig_t *initialnavConfig,
pidProfile_t *initialPidProfile,
rcControlsConfig_t *initialRcControlsConfig,
rxConfig_t * initialRxConfig,
flight3DConfig_t * initialFlight3DConfig,
escAndServoConfig_t * initialEscAndServoConfig)
{
/* Initial state */
@ -2201,6 +2207,7 @@ void navigationInit(navConfig_t *initialnavConfig,
navigationUseRcControlsConfig(initialRcControlsConfig);
navigationUseRxConfig(initialRxConfig);
navigationUseEscAndServoConfig(initialEscAndServoConfig);
navigationUseFlight3DConfig(initialFlight3DConfig);
}
/*-----------------------------------------------------------

5
src/main/flight/navigation_rewrite.h
View file

@ -26,6 +26,7 @@
#include "flight/pid.h"
#include "flight/failsafe.h"
#include "flight/mixer.h"
/* GPS Home location data */
extern gpsLocation_t GPS_home;
@ -68,7 +69,7 @@ enum {
typedef struct navConfig_s {
struct {
uint8_t __stub; // Don't remember throttle when althold was initiated, assume that throttle is at middle = zero climb rate
uint8_t use_thr_mid_for_althold; // Don't remember throttle when althold was initiated, assume that throttle is at Thr Mid = zero climb rate
uint8_t extra_arming_safety; // Forcibly apply 100% throttle tilt compensation
uint8_t user_control_mode; // NAV_GPS_ATTI or NAV_GPS_CRUISE
uint8_t rth_alt_control_style; // Controls how RTH controls altitude
@ -218,10 +219,12 @@ void navigationUseConfig(navConfig_t *navConfigToUse);
void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig);
void navigationUseRxConfig(rxConfig_t * initialRxConfig);
void navigationUseEscAndServoConfig(escAndServoConfig_t * initialEscAndServoConfig);
void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig);
void navigationInit(navConfig_t *initialnavConfig,
pidProfile_t *initialPidProfile,
rcControlsConfig_t *initialRcControlsConfig,
rxConfig_t * initialRxConfig,
flight3DConfig_t * initialFlight3DConfig,
escAndServoConfig_t * initialEscAndServoConfig);
/* Navigation system updates */

38
src/main/flight/navigation_rewrite_multicopter.c
View file

@ -35,6 +35,10 @@
#include "sensors/acceleration.h"
#include "sensors/boardalignment.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/navigation_rewrite.h"
@ -55,6 +59,7 @@ extern int16_t magHold;
* Altitude controller for multicopter aircraft
*-----------------------------------------------------------*/
static int16_t rcCommandAdjustedThrottle;
static int16_t altHoldThrottleRCZero = 1500;
static filterStatePt1_t altholdThrottleFilterState;
/* Calculate global altitude setpoint based on surface setpoint */
@ -99,10 +104,21 @@ static void updateAltitudeThrottleController_MC(uint32_t deltaMicros)
bool adjustMulticopterAltitudeFromRCInput(void)
{
int16_t rcThrottleAdjustment = rcCommand[THROTTLE] - posControl.rxConfig->midrc;
int16_t rcThrottleAdjustment = rcCommand[THROTTLE] - altHoldThrottleRCZero;
if (ABS(rcThrottleAdjustment) > posControl.rcControlsConfig->alt_hold_deadband) {
// set velocity proportional to stick movement
float rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / 500;
float rcClimbRate;
// Make sure we can satisfy max_manual_climb_rate in both up and down directions
if (rcThrottleAdjustment > 0) {
// Scaling from altHoldThrottleRCZero to maxthrottle
rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (posControl.escAndServoConfig->maxthrottle - altHoldThrottleRCZero);
}
else {
// Scaling from minthrottle to altHoldThrottleRCZero
rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (altHoldThrottleRCZero - posControl.escAndServoConfig->minthrottle);
}
updateAltitudeTargetFromClimbRate(rcClimbRate);
return true;
@ -120,6 +136,24 @@ bool adjustMulticopterAltitudeFromRCInput(void)
void setupMulticopterAltitudeController(void)
{
// Nothing here
if (posControl.navConfig->flags.use_thr_mid_for_althold) {
altHoldThrottleRCZero = lookupThrottleRCMid;
}
else {
// If throttle status is THROTTLE_LOW - use Thr Mid anyway
throttleStatus_e throttleStatus = calculateThrottleStatus(posControl.rxConfig, posControl.flight3DConfig->deadband3d_throttle);
if (throttleStatus == THROTTLE_LOW) {
altHoldThrottleRCZero = lookupThrottleRCMid;
}
else {
altHoldThrottleRCZero = rcCommand[THROTTLE];
}
}
// Make sure we are able to satisfy the deadband
altHoldThrottleRCZero = constrain(altHoldThrottleRCZero,
posControl.escAndServoConfig->minthrottle + posControl.rcControlsConfig->alt_hold_deadband + 10,
posControl.escAndServoConfig->maxthrottle - posControl.rcControlsConfig->alt_hold_deadband - 10);
}
void resetMulticopterAltitudeController()

2
src/main/flight/navigation_rewrite_private.h
View file

@ -62,6 +62,7 @@ typedef struct navigationFlags_s {
// Behaviour modifiers
bool isGCSAssistedNavigationEnabled; // Does iNav accept WP#255 - follow-me etc.
//bool isTerrainFollowEnabled; // Does iNav use sonar for terrain following (adjusting baro altitude target according to sonar readings)
bool forcedRTHActivated;
} navigationFlags_t;
@ -259,6 +260,7 @@ typedef struct {
rcControlsConfig_t * rcControlsConfig;
pidProfile_t * pidProfile;
rxConfig_t * rxConfig;
flight3DConfig_t * flight3DConfig;
escAndServoConfig_t * escAndServoConfig;
} navigationPosControl_t;

4
src/main/io/rc_curves.c
View file

@ -27,7 +27,7 @@
int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW
int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
int16_t lookupThrottleRCMid; // THROTTLE curve mid point
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig)
{
@ -49,6 +49,8 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo
{
uint8_t i;
lookupThrottleRCMid = escAndServoConfig->minthrottle + (int32_t)(escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle) * controlRateConfig->thrMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE]
for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
int16_t tmp = 10 * i - controlRateConfig->thrMid8;
uint8_t y = 1;

1
src/main/io/rc_curves.h
View file

@ -23,6 +23,7 @@
extern int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
extern int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW
extern int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
extern int16_t lookupThrottleRCMid; // THROTTLE curve mid
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig);
void generateYawCurve(controlRateConfig_t *controlRateConfig);

1
src/main/io/serial_cli.c
View file

@ -586,6 +586,7 @@ const clivalue_t valueTable[] = {
{ "inav_max_eph_epv", VAR_FLOAT | MASTER_VALUE, &masterConfig.navConfig.inav.max_eph_epv, .config.minmax = { 0, 9999 }, 0 },
{ "inav_baro_epv", VAR_FLOAT | MASTER_VALUE, &masterConfig.navConfig.inav.baro_epv, .config.minmax = { 0, 9999 }, 0 },
{ "nav_use_midthr_for_althold", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.use_thr_mid_for_althold, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "nav_extra_arming_safety", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.extra_arming_safety, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "nav_user_control_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.user_control_mode, .config.lookup = { TABLE_NAV_USER_CTL_MODE }, 0 },
{ "nav_position_timeout", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.pos_failure_timeout, .config.minmax = { 0, 10 }, 0 },

1
src/main/main.c
View file

@ -452,6 +452,7 @@ void init(void)
&currentProfile->pidProfile,
&currentProfile->rcControlsConfig,
&masterConfig.rxConfig,
&masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig
);
#endif