Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

Shared altitude control function in position_control.c (#13954)

Browse source 
* Altitude control code shared

fewer debugs
subtract D

* remove #include comments, simplify coding, restructuring

thanks JP and MH

* fix cms limits for throttle control

* Use altitude_control debug, fix throttle calculation

minor refactoring

* use AUTO_CONTROL_ALTITUDE debug in place of GPS Rescue throttle pid

* use autopilot for position control names

* fixes from reviews - thanks

* Re-organise included files and functions thanks Karate

* missed init and other typos

* remove old unused unit test file, tidy up thanks Mark

* fix indentation on one line
This commit is contained in:
ctzsnooze 2024-10-17 08:17:44 +11:00 • committed by GitHub
parent 1d9823b4cd
commit 58fc8bbbb8
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
33 changed files with 263 additions and 595 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/test/Makefile
View file

@ -43,8 +43,10 @@ arming_prevention_unittest_SRC := \
$(USER_DIR)/fc/rc_controls.c \
$(USER_DIR)/fc/rc_modes.c \
$(USER_DIR)/fc/runtime_config.c \
$(USER_DIR)/flight/autopilot.c \
$(USER_DIR)/flight/gps_rescue.c
arming_prevention_unittest_DEFINES := \
USE_GPS_RESCUE=
@ -466,6 +468,7 @@ rx_spi_expresslrs_telemetry_unittest_DEFINES := \
althold_unittest_SRC := \
$(USER_DIR)/flight/alt_hold.c \
$(USER_DIR)/flight/autopilot.c \
$(USER_DIR)/common/maths.c \
$(USER_DIR)/pg/rx.c

34
src/test/unit/althold_unittest.cc
View file

@ -31,11 +31,11 @@ extern "C" {
#include "fc/runtime_config.h"
#include "flight/alt_hold.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/position.h"
#include "flight/position_control.h"
#include "flight/pid.h"
#include "flight/position.h"
#include "rx/rx.h"
@ -43,12 +43,10 @@ extern "C" {
PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
PG_REGISTER(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 0);
extern altHoldState_t altHoldState;
void altHoldReset(void);
void altHoldProcessTransitions(void);
void altHoldInit(void);
void updateAltHoldState(timeUs_t);
bool failsafeIsActive(void) { return false; }
@ -100,33 +98,11 @@ TEST(AltholdUnittest, altHoldTransitionsTestUnfinishedExitEnter)
extern "C" {
acc_t acc;
pidCoefficient_t testAltitudePidCoeffs = {15.0f, 15.0f, 15.1f, 15.0f};
const pidCoefficient_t *getAltitudePidCoeffs(void) {
return &testAltitudePidCoeffs;
}
float getAltitudeCm(void) { return 0.0f;}
float getAltitudeDerivative(void) { return 0.0f;}
void pt2FilterInit(pt2Filter_t *altHoldDeltaLpf, float) {
UNUSED(altHoldDeltaLpf);
}
float pt2FilterGain(float, float) {
return 0.0f;
}
float pt2FilterApply(pt2Filter_t *altHoldDeltaLpf, float) {
UNUSED(altHoldDeltaLpf);
return 0.0f;
}
bool isAltitudeAvailable(void) { return true; }
float getAltitudeCm(void) {return 0.0f;}
float getAltitudeDerivative(void) {return 0.0f;}
float getCosTiltAngle(void) { return 0.0f; }
float rcCommand[4];
float getRcDeflection(int)
{
return 0;
}
void parseRcChannels(const char *input, rxConfig_t *rxConfig)
{
UNUSED(input);

196
src/test/unit/altitude_hold_unittest.cc.txt
View file

@ -1,196 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <stdbool.h>
#include <limits.h>
//#define DEBUG_ALTITUDE_HOLD
#define USE_BARO
extern "C" {
#include "debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "rx/rx.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/position.h"
#include "config/runtime_config.h"
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
#define DOWNWARDS_THRUST true
#define UPWARDS_THRUST false
extern "C" {
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations);
uint16_t calculateTiltAngle(rollAndPitchInclination_t *inclinations);
}
typedef struct inclinationExpectation_s {
rollAndPitchInclination_t inclination;
bool expectDownwardsThrust;
} inclinationExpectation_t;
TEST(AltitudeHoldTest, IsThrustFacingDownwards)
{
// given
inclinationExpectation_t inclinationExpectations[] = {
{ {{ 0, 0 }}, DOWNWARDS_THRUST },
{ {{ 799, 799 }}, DOWNWARDS_THRUST },
{ {{ 800, 799 }}, UPWARDS_THRUST },
{ {{ 799, 800 }}, UPWARDS_THRUST },
{ {{ 800, 800 }}, UPWARDS_THRUST },
{ {{ 801, 801 }}, UPWARDS_THRUST },
{ {{ -799, -799 }}, DOWNWARDS_THRUST },
{ {{ -800, -799 }}, UPWARDS_THRUST },
{ {{ -799, -800 }}, UPWARDS_THRUST },
{ {{ -800, -800 }}, UPWARDS_THRUST },
{ {{ -801, -801 }}, UPWARDS_THRUST }
};
uint8_t testIterationCount = sizeof(inclinationExpectations) / sizeof(inclinationExpectation_t);
// expect
for (uint8_t index = 0; index < testIterationCount; index ++) {
inclinationExpectation_t *angleInclinationExpectation = &inclinationExpectations[index];
#ifdef DEBUG_ALTITUDE_HOLD
printf("iteration: %d\n", index);
#endif
bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination);
EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result);
}
}
typedef struct inclinationAngleExpectations_s {
rollAndPitchInclination_t inclination;
uint16_t expected_angle;
} inclinationAngleExpectations_t;
TEST(AltitudeHoldTest, TestCalculateTiltAngle)
{
inclinationAngleExpectations_t inclinationAngleExpectations[] = {
{ {{ 0, 0}}, 0},
{ {{ 1, 0}}, 1},
{ {{ 0, 1}}, 1},
{ {{ 0, -1}}, 1},
{ {{-1, 0}}, 1},
{ {{-1, -2}}, 2},
{ {{-2, -1}}, 2},
{ {{ 1, 2}}, 2},
{ {{ 2, 1}}, 2}
};
rollAndPitchInclination_t inclination = {{0, 0}};
uint16_t tilt_angle = calculateTiltAngle(&inclination);
EXPECT_EQ(tilt_angle, 0);
for (uint8_t i = 0; i < 9; i++) {
inclinationAngleExpectations_t *expectation = &inclinationAngleExpectations[i];
uint16_t result = calculateTiltAngle(&expectation->inclination);
EXPECT_EQ(expectation->expected_angle, result);
}
}
// STUBS
extern "C" {
uint32_t rcModeActivationMask;
float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint32_t accTimeSum ; // keep track for integration of acc
int accSumCount;
float accVelScale;
rollAndPitchInclination_t inclination;
//uint16_t acc_1G;
//int16_t heading;
//gyro_t gyro;
int32_t accSum[XYZ_AXIS_COUNT];
//int16_t magADC[XYZ_AXIS_COUNT];
int32_t BaroAlt;
int16_t debug[DEBUG16_VALUE_COUNT];
uint8_t stateFlags;
uint16_t flightModeFlags;
uint8_t armingFlags;
int32_t sonarAlt;
uint16_t enableFlightMode(flightModeFlags_e mask)
{
return flightModeFlags |= (mask);
}
uint16_t disableFlightMode(flightModeFlags_e mask)
{
return flightModeFlags &= ~(mask);
}
void gyroUpdate(void) {};
bool sensors(uint32_t mask)
{
UNUSED(mask);
return false;
};
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
UNUSED(rollAndPitchTrims);
}
void imuResetAccelerationSum(void) {};
int32_t applyDeadband(int32_t, int32_t) { return 0; }
uint32_t micros(void) { return 0; }
bool isBaroCalibrationComplete(void) { return true; }
void performBaroCalibrationCycle(void) {}
int32_t baroCalculateAltitude(void) { return 0; }
int constrain(int amt, int low, int high)
{
UNUSED(amt);
UNUSED(low);
UNUSED(high);
return 0;
}
}

9
src/test/unit/arming_prevention_unittest.cc
View file

@ -36,13 +36,13 @@ extern "C" {
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h"
#include "flight/gps_rescue.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/position.h"
#include "flight/position_control.h"
#include "flight/servos.h"
#include "io/beeper.h"
@ -78,7 +78,7 @@ extern "C" {
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint16_t averageSystemLoadPercent = 0;
@ -1137,10 +1137,6 @@ extern "C" {
void pinioBoxTaskControl(void) {}
void schedulerSetNextStateTime(timeDelta_t) {}
pidCoefficient_t testAltitudePidCoeffs = {15.0f, 15.0f, 15.1f, 15.0f};
const pidCoefficient_t *getAltitudePidCoeffs(void) {
return &testAltitudePidCoeffs;
}
float getAltitudeCm(void) {return 0.0f;}
float getAltitudeDerivative(void) {return 0.0f;}
@ -1164,5 +1160,4 @@ extern "C" {
void getRcDeflectionAbs(void) {}
uint32_t getCpuPercentageLate(void) { return 0; }
bool crashFlipSuccessful(void) { return false; }
bool isBelowLandingAltitude(void) { return false; };
}

8
src/test/unit/flight_imu_unittest.cc
View file

@ -41,12 +41,12 @@ extern "C" {
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "fc/rc.h"
#include "flight/autopilot.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/position.h"
#include "flight/position_control.h"
#include "io/gps.h"
@ -74,7 +74,7 @@ extern "C" {
PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0);
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
.enabledFeatures = 0