rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'nebbian-bugfix-sonar-drift'

This commit is contained in:
Dominic Clifton 2015-04-14 19:55:23 +01:00
parent 1a2ef223a0 6eaca17cbd
commit 5d62f6da69
3 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/main/flight/altitudehold.c
View File

@ -89,10 +89,10 @@ static void applyMultirotorAltHold(void)
// multirotor alt hold
if (rcControlsConfig->alt_hold_fast_change) {
// rapid alt changes
if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
errorVelocityI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband;
rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
@ -102,9 +102,9 @@ static void applyMultirotorAltHold(void)
}
} else {
// slow alt changes, mostly used for aerial photography
if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2;
setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2;
velocityControl = 1;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
@ -145,7 +145,7 @@ void updateAltHoldState(void)
if (!FLIGHT_MODE(BARO_MODE)) {
ENABLE_FLIGHT_MODE(BARO_MODE);
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
initialThrottleHold = rcData[THROTTLE];
errorVelocityI = 0;
altHoldThrottleAdjustment = 0;
}
@ -162,7 +162,7 @@ void updateSonarAltHoldState(void)
if (!FLIGHT_MODE(SONAR_MODE)) {
ENABLE_FLIGHT_MODE(SONAR_MODE);
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
initialThrottleHold = rcData[THROTTLE];
errorVelocityI = 0;
altHoldThrottleAdjustment = 0;
}

1
src/test/unit/altitude_hold_unittest.cc
View File

@ -134,6 +134,7 @@ TEST(AltitudeHoldTest, TestCalculateTiltAngle)
extern "C" {
uint32_t rcModeActivationMask;
int16_t rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint32_t accTimeSum ; // keep track for integration of acc
int accSumCount;

3
src/test/unit/flight_imu_unittest.cc
View File

@ -39,6 +39,8 @@ extern "C" {
#include "config/runtime_config.h"
#include "rx/rx.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
@ -75,6 +77,7 @@ TEST(FlightImuTest, TestCalculateHeading)
extern "C" {
uint32_t rcModeActivationMask;
int16_t rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint16_t acc_1G;
int16_t heading;