Adding test for the downwards thrust direction logic added in
495c6b8f73.This commit is contained in:
Dominic Clifton
parent
f268c9c4f2
commit
ddd322fb9f
|
|
@ -68,7 +68,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
for (axis = 0; axis < 3; axis++) {
|
||||
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
|
||||
// 50 degrees max inclination
|
||||
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim->raw[axis];
|
||||
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle.raw[axis] + angleTrim->raw[axis];
|
||||
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
|
||||
PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
|
||||
|
||||
|
|
@ -127,7 +127,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
|||
// -----Get the desired angle rate depending on flight mode
|
||||
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { // MODE relying on ACC
|
||||
// calculate error and limit the angle to max configured inclination
|
||||
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim->raw[axis]; // 16 bits is ok here
|
||||
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
|
||||
}
|
||||
if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
|
||||
AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5);
|
||||
|
|
|
|||
|
|
@ -28,8 +28,6 @@ enum {
|
|||
|
||||
#define ANGLE_INDEX_COUNT 2
|
||||
|
||||
extern int16_t angle[ANGLE_INDEX_COUNT]; // see angle_index_t
|
||||
|
||||
// See http://en.wikipedia.org/wiki/Flight_dynamics
|
||||
enum {
|
||||
FD_ROLL = 0,
|
||||
|
|
@ -82,6 +80,20 @@ typedef union {
|
|||
rollAndPitchTrims_t_def trims;
|
||||
} rollAndPitchTrims_t;
|
||||
|
||||
typedef struct angleInclinations_s {
|
||||
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
||||
int16_t rollDeciDegrees;
|
||||
int16_t pitchDeciDegrees;
|
||||
} angleInclinations_t_def;
|
||||
|
||||
typedef union {
|
||||
int16_t raw[ANGLE_INDEX_COUNT];
|
||||
angleInclinations_t_def angles;
|
||||
} angleInclinations_t;
|
||||
|
||||
|
||||
extern angleInclinations_t angle;
|
||||
|
||||
extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
|
||||
extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "platform.h"
|
||||
#include <platform.h>
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "flight_common.h"
|
||||
|
|
@ -68,7 +68,7 @@ float magneticDeclination = 0.0f; // calculated at startup from config
|
|||
int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||
|
||||
int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
||||
angleInclinations_t angle = { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
||||
float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
|
||||
|
||||
static void getEstimatedAttitude(void);
|
||||
|
|
@ -324,8 +324,8 @@ static void getEstimatedAttitude(void)
|
|||
// Attitude of the estimated vector
|
||||
anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
|
||||
anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
|
||||
angle[AI_ROLL] = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
|
||||
angle[AI_PITCH] = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
|
||||
angle.angles.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
|
||||
angle.angles.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
|
||||
|
||||
if (sensors(SENSOR_MAG))
|
||||
heading = calculateHeading(&EstM);
|
||||
|
|
@ -353,6 +353,14 @@ static void getEstimatedAttitude(void)
|
|||
#ifdef BARO
|
||||
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
|
||||
|
||||
|
||||
#define DEGREES_80_IN_DECIDEGREES 800
|
||||
|
||||
bool isThrustFacingDownwards(angleInclinations_t *angleInclinations)
|
||||
{
|
||||
return abs(angleInclinations->angles.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(angleInclinations->angles.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
|
||||
}
|
||||
|
||||
int getEstimatedAltitude(void)
|
||||
{
|
||||
static uint32_t previousT;
|
||||
|
|
@ -415,7 +423,9 @@ int getEstimatedAltitude(void)
|
|||
|
||||
// set vario
|
||||
vario = applyDeadband(vel_tmp, 5);
|
||||
if (abs(angle[ROLL]) < 800 && abs(angle[PITCH]) < 800) { // only calculate pid if the copters thrust is facing downwards(<80deg)
|
||||
|
||||
// only calculate pid if the copters thrust is facing downwards
|
||||
if (isThrustFacingDownwards(&angle)) {
|
||||
// Altitude P-Controller
|
||||
error = constrain(AltHold - EstAlt, -500, 500);
|
||||
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
|
||||
|
|
|
|||
|
|
@ -448,8 +448,8 @@ void mixTable(void)
|
|||
break;
|
||||
|
||||
case MULTITYPE_GIMBAL:
|
||||
servo[0] = (((int32_t)servoConf[0].rate * angle[PITCH]) / 50) + determineServoMiddleOrForwardFromChannel(0);
|
||||
servo[1] = (((int32_t)servoConf[1].rate * angle[ROLL]) / 50) + determineServoMiddleOrForwardFromChannel(1);
|
||||
servo[0] = (((int32_t)servoConf[0].rate * angle.angles.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(0);
|
||||
servo[1] = (((int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(1);
|
||||
break;
|
||||
|
||||
case MULTITYPE_AIRPLANE:
|
||||
|
|
@ -502,11 +502,11 @@ void mixTable(void)
|
|||
|
||||
if (rcOptions[BOXCAMSTAB]) {
|
||||
if (gimbalConfig->gimbal_flags & GIMBAL_MIXTILT) {
|
||||
servo[0] -= (-(int32_t)servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)servoConf[1].rate * angle[ROLL] / 50;
|
||||
servo[1] += (-(int32_t)servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)servoConf[1].rate * angle[ROLL] / 50;
|
||||
servo[0] -= (-(int32_t)servoConf[0].rate) * angle.angles.pitchDeciDegrees / 50 - (int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees / 50;
|
||||
servo[1] += (-(int32_t)servoConf[0].rate) * angle.angles.pitchDeciDegrees / 50 + (int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees / 50;
|
||||
} else {
|
||||
servo[0] += (int32_t)servoConf[0].rate * angle[PITCH] / 50;
|
||||
servo[1] += (int32_t)servoConf[1].rate * angle[ROLL] / 50;
|
||||
servo[0] += (int32_t)servoConf[0].rate * angle.angles.pitchDeciDegrees / 50;
|
||||
servo[1] += (int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees / 50;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
2
src/mw.c
2
src/mw.c
|
|
@ -173,7 +173,7 @@ void annexCode(void)
|
|||
static uint32_t LEDTime;
|
||||
if ((int32_t)(currentTime - LEDTime) >= 0) {
|
||||
LEDTime = currentTime + 50000;
|
||||
ledringState(f.ARMED, angle[AI_PITCH], angle[AI_ROLL]);
|
||||
ledringState(f.ARMED, angle.angles.pitchDeciDegrees, angle.angles.rollDeciDegrees);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -33,7 +33,9 @@
|
|||
|
||||
#define SENSORS_SET (SENSOR_ACC)
|
||||
|
||||
#else
|
||||
#endif // STM32F3DISCOVERY
|
||||
|
||||
#ifdef STM32F10X_MD
|
||||
|
||||
#include "stm32f10x_conf.h"
|
||||
#include "core_cm3.h"
|
||||
|
|
@ -108,4 +110,4 @@
|
|||
#endif
|
||||
#endif
|
||||
|
||||
#endif
|
||||
#endif // STM32F10X_MD
|
||||
|
|
|
|||
|
|
@ -521,7 +521,7 @@ static void evaluateCommand(void)
|
|||
case MSP_ATTITUDE:
|
||||
headSerialReply(6);
|
||||
for (i = 0; i < 2; i++)
|
||||
serialize16(angle[i]);
|
||||
serialize16(angle.raw[i]);
|
||||
serialize16(heading);
|
||||
break;
|
||||
case MSP_ALTITUDE:
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ CXXFLAGS += -g -Wall -Wextra -pthread
|
|||
|
||||
# All tests produced by this Makefile. Remember to add new tests you
|
||||
# created to the list.
|
||||
TESTS = battery_unittest
|
||||
TESTS = battery_unittest flight_imu_unittest
|
||||
|
||||
# All Google Test headers. Usually you shouldn't change this
|
||||
# definition.
|
||||
|
|
@ -71,6 +71,8 @@ gtest_main.a : gtest-all.o gtest_main.o
|
|||
# gtest_main.a, depending on whether it defines its own main()
|
||||
# function.
|
||||
|
||||
|
||||
|
||||
battery.o : $(USER_DIR)/battery.c $(USER_DIR)/battery.h $(GTEST_HEADERS)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $(USER_DIR)/battery.c
|
||||
|
||||
|
|
@ -81,3 +83,16 @@ battery_unittest.o : $(TEST_DIR)/battery_unittest.cc \
|
|||
battery_unittest : battery.o battery_unittest.o gtest_main.a
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $@
|
||||
|
||||
|
||||
|
||||
|
||||
flight_imu.o : $(USER_DIR)/flight_imu.c $(USER_DIR)/flight_imu.h $(GTEST_HEADERS)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $(USER_DIR)/flight_imu.c -I$(TEST_DIR)
|
||||
|
||||
flight_imu_unittest.o : $(TEST_DIR)/flight_imu_unittest.cc \
|
||||
$(USER_DIR)/flight_imu.h $(GTEST_HEADERS)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $(TEST_DIR)/flight_imu_unittest.cc -I$(USER_DIR)
|
||||
|
||||
flight_imu_unittest : flight_imu.o flight_imu_unittest.o gtest_main.a
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $@
|
||||
|
||||
|
|
|
|||
|
|
@ -2,6 +2,8 @@
|
|||
|
||||
#include <limits.h>
|
||||
#include "battery.h"
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
typedef struct batteryAdcToVoltageExpectation_s {
|
||||
|
|
@ -44,9 +46,6 @@ TEST(BatteryTest, BatteryADCToVoltage)
|
|||
|
||||
// STUBS
|
||||
|
||||
#define UNUSED(x) (void)(x)
|
||||
|
||||
|
||||
uint16_t adcGetChannel(uint8_t channel)
|
||||
{
|
||||
UNUSED(channel);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,128 @@
|
|||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
#define BARO
|
||||
|
||||
|
||||
// FIXME this giant list of includes (below) and stubs (bottom) indicates there is too much going on in flight_imu.c and that it needs decoupling and breaking up.
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "flight_common.h"
|
||||
|
||||
#include "sensors_common.h"
|
||||
#include "drivers/accgyro_common.h"
|
||||
#include "sensors_gyro.h"
|
||||
#include "sensors_compass.h"
|
||||
#include "sensors_acceleration.h"
|
||||
#include "sensors_barometer.h"
|
||||
|
||||
#include "gps_common.h"
|
||||
|
||||
#include "gimbal.h"
|
||||
#include "flight_mixer.h"
|
||||
|
||||
// FIXME remove dependency on config.h
|
||||
#include "boardalignment.h"
|
||||
#include "battery.h"
|
||||
#include "escservo.h"
|
||||
#include "rc_controls.h"
|
||||
#include "rx_common.h"
|
||||
#include "telemetry_common.h"
|
||||
#include "drivers/serial_common.h"
|
||||
#include "serial_common.h"
|
||||
#include "failsafe.h"
|
||||
#include "runtime_config.h"
|
||||
#include "config.h"
|
||||
#include "config_profile.h"
|
||||
#include "config_master.h"
|
||||
|
||||
|
||||
#include "flight_imu.h"
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
#define DOWNWARDS_THRUST true
|
||||
#define UPWARDS_THRUST false
|
||||
|
||||
|
||||
bool isThrustFacingDownwards(angleInclinations_t *angleInclinations);
|
||||
|
||||
typedef struct angleInclinationExpectation_s {
|
||||
angleInclinations_t inclination;
|
||||
bool expectDownwardsThrust;
|
||||
} angleInclinationExpectation_t;
|
||||
|
||||
TEST(FlightImuTest, IsThrustFacingDownwards)
|
||||
{
|
||||
// given
|
||||
|
||||
angleInclinationExpectation_t angleInclinationExpectations[] = {
|
||||
{ { 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(angleInclinationExpectations) / sizeof(angleInclinationExpectation_t);
|
||||
|
||||
// expect
|
||||
|
||||
for (uint8_t index = 0; index < testIterationCount; index ++) {
|
||||
angleInclinationExpectation_t *angleInclinationExpectation = &angleInclinationExpectations[index];
|
||||
printf("iteration: %d\n", index);
|
||||
bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination);
|
||||
EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(FlightImuTest, IsThrustFacingUpwards)
|
||||
{
|
||||
angleInclinations_t angleInclinations = { { 800, 800 } };
|
||||
|
||||
bool result = isThrustFacingDownwards(&angleInclinations);
|
||||
|
||||
EXPECT_EQ(false, result);
|
||||
}
|
||||
|
||||
// STUBS
|
||||
|
||||
uint16_t acc_1G;
|
||||
profile_t currentProfile;
|
||||
master_t masterConfig;
|
||||
int16_t heading;
|
||||
flags_t f;
|
||||
gyro_t gyro;
|
||||
int16_t magADC[XYZ_AXIS_COUNT];
|
||||
int32_t BaroAlt;
|
||||
|
||||
void gyroGetADC(void) {};
|
||||
bool sensors(uint32_t mask)
|
||||
{
|
||||
UNUSED(mask);
|
||||
return false;
|
||||
};
|
||||
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
|
||||
{
|
||||
UNUSED(rollAndPitchTrims);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
#pragma once
|
||||
|
||||
|
||||
#define UNUSED(x) (void)(x)
|
||||
Loading…
Reference in New Issue