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

Decouple altitudehold.c from config.c. Update flight_imu_unittest and 

altitude_hold_unittest.
This commit is contained in:
Dominic Clifton 2014-11-03 20:03:21 +01:00
parent f8b13d7c62
commit 503e7a0817
15 changed files with 243 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/main/config/config.c
View File

@ -248,7 +248,13 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->thrExpo8 = 0;
controlRateConfig->dynThrPID = 0;
controlRateConfig->tpa_breakpoint = 1500;
}
void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
rcControlsConfig->deadband = 0;
rcControlsConfig->yaw_deadband = 0;
rcControlsConfig->alt_hold_deadband = 40;
rcControlsConfig->alt_hold_fast_change = 1;
}
uint8_t getCurrentProfile(void)
@ -330,7 +336,7 @@ static void resetConf(void)
masterConfig.small_angle = 25;
masterConfig.airplaneConfig.flaps_speed = 0;
masterConfig.fixedwing_althold_dir = 1;
masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
// Motor/ESC/Servo
resetEscAndServoConfig(&masterConfig.escAndServoConfig);
@ -377,10 +383,9 @@ static void resetConf(void)
// Radio
parseRcChannels("AETR1234", &masterConfig.rxConfig);
currentProfile->deadband = 0;
currentProfile->yaw_deadband = 0;
currentProfile->alt_hold_deadband = 40;
currentProfile->alt_hold_fast_change = 1;
resetRcControlsConfig(&currentProfile->rcControlsConfig);
currentProfile->throttle_correction_value = 0; // could 10 with althold or 40 for fpv
currentProfile->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
@ -508,7 +513,7 @@ void activateConfig(void)
imuRuntimeConfig.small_angle = masterConfig.small_angle;
configureImu(&imuRuntimeConfig, &currentProfile->pidProfile, &currentProfile->accDeadband);
configureAltitudeHold(&currentProfile->pidProfile, &currentProfile->barometerConfig);
configureAltitudeHold(&currentProfile->pidProfile, &currentProfile->barometerConfig, &currentProfile->rcControlsConfig, &masterConfig.escAndServoConfig);
calculateThrottleAngleScale(currentProfile->throttle_correction_angle);
calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff);

1
src/main/config/config_master.h
View File

@ -64,7 +64,6 @@ typedef struct master_t {
uint8_t small_angle;
airplaneConfig_t airplaneConfig;
int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign
#ifdef GPS
gpsConfig_t gpsConfig;

7
src/main/config/config_profile.h
View File

@ -36,7 +36,6 @@ typedef struct profile_s {
barometerConfig_t barometerConfig;
uint8_t acc_unarmedcal; // turn automatic acc compensation on/off
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
@ -44,10 +43,8 @@ typedef struct profile_s {
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
// Radio/ESC-related configuration
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t alt_hold_deadband; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement
rcControlsConfig_t rcControlsConfig;
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.

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

@ -21,7 +21,6 @@
#include <stdlib.h>
#include <math.h>
#include "common/maths.h"
#include "platform.h"
@ -29,51 +28,48 @@
#include "common/axis.h"
#include "drivers/accgyro.h"
#include "drivers/serial.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
// FIXME remove dependency on currentProfile and masterConfig globals and clean up include file list.
#include "common/color.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sonar.h"
#include "io/escservo.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/serial.h"
#include "io/ledstrip.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "telemetry/telemetry.h"
#include "flight/imu.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "io/escservo.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
int32_t setVelocity = 0;
uint8_t velocityControl = 0;
int32_t errorVelocityI = 0;
int32_t altHoldThrottleAdjustment = 0;
int32_t AltHold;
int32_t EstAlt; // in cm
int32_t vario = 0; // variometer in cm/s
barometerConfig_t *barometerConfig;
pidProfile_t *pidProfile;
rcControlsConfig_t *rcControlsConfig;
escAndServoConfig_t *escAndServoConfig;
void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig)
void configureAltitudeHold(
pidProfile_t *initialPidProfile,
barometerConfig_t *intialBarometerConfig,
rcControlsConfig_t *initialRcControlsConfig,
escAndServoConfig_t *initialEscAndServoConfig
)
{
pidProfile = initialPidProfile;
barometerConfig = intialBarometerConfig;
rcControlsConfig = initialRcControlsConfig;
escAndServoConfig = initialEscAndServoConfig;
}
#if defined(BARO) || defined(SONAR)
@ -90,22 +86,22 @@ static void applyMultirotorAltHold(void)
{
static uint8_t isAltHoldChanged = 0;
// multirotor alt hold
if (currentProfile->alt_hold_fast_change) {
if (rcControlsConfig->alt_hold_fast_change) {
// rapid alt changes
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile->alt_hold_deadband) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
errorVelocityI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile->alt_hold_deadband : currentProfile->alt_hold_deadband;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
}
} else {
// slow alt changes, mostly used for aerial photography
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile->alt_hold_deadband) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2;
velocityControl = 1;
@ -115,23 +111,23 @@ static void applyMultirotorAltHold(void)
velocityControl = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
}
}
static void applyFixedWingAltHold()
static void applyFixedWingAltHold(airplaneConfig_t *airplaneConfig)
{
// handle fixedwing-related althold. UNTESTED! and probably wrong
// most likely need to check changes on pitch channel and 'reset' althold similar to
// how throttle does it on multirotor
rcCommand[PITCH] += altHoldThrottleAdjustment * masterConfig.fixedwing_althold_dir;
rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig->fixedwing_althold_dir;
}
void applyAltHold(void)
void applyAltHold(airplaneConfig_t *airplaneConfig)
{
if (STATE(FIXED_WING)) {
applyFixedWingAltHold();
applyFixedWingAltHold(airplaneConfig);
} else {
applyMultirotorAltHold();
}

9
src/main/flight/altitudehold.h
View File

@ -15,9 +15,14 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig);
//extern int32_t errorVelocityI;
//extern int32_t altHoldThrottleAdjustment;
//extern uint8_t velocityControl;
//extern int32_t setVelocity;
void applyAltHold(void);
void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig);
void applyAltHold(airplaneConfig_t *airplaneConfig);
void updateAltHoldState(void);
void updateSonarAltHoldState(void);

1
src/main/flight/flight.h
View File

@ -129,7 +129,6 @@ extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int16_t heading, magHold;
extern int32_t EstAlt;
extern int32_t AltHold;
extern int32_t EstAlt;
extern int32_t vario;

10
src/main/flight/imu.c
View File

@ -54,19 +54,9 @@ float accVelScale;
int16_t smallAngle = 0;
int32_t EstAlt; // in cm
int32_t AltHold;
int32_t setVelocity = 0;
uint8_t velocityControl = 0;
int32_t errorVelocityI = 0;
int32_t vario = 0; // variometer in cm/s
float throttleAngleScale;
float fc_acc;
int32_t altHoldThrottleAdjustment = 0;
float magneticDeclination = 0.0f; // calculated at startup from config
float gyroScaleRad;

4
src/main/flight/imu.h
View File

@ -17,10 +17,6 @@
#pragma once
extern int32_t errorVelocityI;
extern uint8_t velocityControl;
extern int32_t setVelocity;
extern int32_t altHoldThrottleAdjustment;
extern int16_t throttleAngleCorrection;
extern uint32_t accTimeSum;
extern int accSumCount;

1
src/main/flight/mixer.h
View File

@ -77,6 +77,7 @@ typedef struct flight3DConfig_s {
typedef struct airplaneConfig_t {
uint8_t flaps_speed; // airplane mode flaps, 0 = no flaps, > 0 = flap speed, larger = faster
int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign
} airplaneConfig_t;
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF

7
src/main/io/rc_controls.h
View File

@ -128,6 +128,13 @@ typedef struct controlRateConfig_s {
extern int16_t rcCommand[4];
typedef struct rcControlsConfig_s {
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t alt_hold_deadband; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement
} rcControlsConfig_t;
bool areSticksInApModePosition(uint16_t ap_mode);
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch);

11
src/main/io/serial_cli.c
View File

@ -224,7 +224,7 @@ const clivalue_t valueTable[] = {
{ "flaps_speed", VAR_UINT8 | MASTER_VALUE, &masterConfig.airplaneConfig.flaps_speed, 0, 100 },
{ "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &masterConfig.fixedwing_althold_dir, -1, 1 },
{ "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &masterConfig.airplaneConfig.fixedwing_althold_dir, -1, 1 },
{ "serial_port_1_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[0], 0, SERIAL_PORT_SCENARIO_MAX },
{ "serial_port_2_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[1], 0, SERIAL_PORT_SCENARIO_MAX },
@ -301,16 +301,15 @@ const clivalue_t valueTable[] = {
{ "gyro_cmpf_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpf_factor, 100, 1000 },
{ "gyro_cmpfm_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpfm_factor, 100, 1000 },
{ "alt_hold_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].alt_hold_deadband, 1, 250 },
{ "alt_hold_fast_change", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].alt_hold_fast_change, 0, 1 },
{ "alt_hold_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_deadband, 1, 250 },
{ "alt_hold_fast_change", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_fast_change, 0, 1 },
{ "deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.deadband, 0, 32 },
{ "yaw_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.yaw_deadband, 0, 100 },
{ "throttle_correction_value", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].throttle_correction_value, 0, 150 },
{ "throttle_correction_angle", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].throttle_correction_angle, 1, 900 },
{ "deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].deadband, 0, 32 },
{ "yaw_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].yaw_deadband, 0, 100 },
{ "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 },
{ "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 },
{ "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 },

22
src/main/mw.c
View File

@ -47,11 +47,14 @@
#include "io/beeper.h"
#include "io/display.h"
#include "io/escservo.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
#include "flight/mixer.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/autotune.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "io/gimbal.h"
#include "io/gps.h"
@ -59,9 +62,6 @@
#include "io/serial_cli.h"
#include "io/serial.h"
#include "io/statusindicator.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
#include "rx/msp.h"
#include "telemetry/telemetry.h"
@ -178,9 +178,9 @@ void annexCode(void)
for (axis = 0; axis < 3; axis++) {
tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (currentProfile->deadband) {
if (tmp > currentProfile->deadband) {
tmp -= currentProfile->deadband;
if (currentProfile->rcControlsConfig.deadband) {
if (tmp > currentProfile->rcControlsConfig.deadband) {
tmp -= currentProfile->rcControlsConfig.deadband;
} else {
tmp = 0;
}
@ -191,9 +191,9 @@ void annexCode(void)
prop1 = 100 - (uint16_t)currentControlRateProfile->rollPitchRate * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
} else if (axis == YAW) {
if (currentProfile->yaw_deadband) {
if (tmp > currentProfile->yaw_deadband) {
tmp -= currentProfile->yaw_deadband;
if (currentProfile->rcControlsConfig.yaw_deadband) {
if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
tmp -= currentProfile->rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
@ -636,7 +636,7 @@ void loop(void)
#if defined(BARO) || defined(SONAR)
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
applyAltHold();
applyAltHold(&masterConfig.airplaneConfig);
}
}
#endif

20
src/test/Makefile
View File

@ -36,6 +36,7 @@ CXXFLAGS += -g -Wall -Wextra -pthread -ggdb3 -O0 -DUNIT_TEST
TESTS = \
battery_unittest \
flight_imu_unittest \
altitude_hold_unittest \
gps_conversion_unittest \
telemetry_hott_unittest \
rc_controls_unittest \
@ -105,10 +106,6 @@ battery_unittest : $(OBJECT_DIR)/sensors/battery.o $(OBJECT_DIR)/battery_unittes
$(OBJECT_DIR)/flight/altitudehold.o : $(USER_DIR)/flight/altitudehold.c $(USER_DIR)/flight/altitudehold.h $(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/altitudehold.c -o $@
$(OBJECT_DIR)/flight/imu.o : $(USER_DIR)/flight/imu.c $(USER_DIR)/flight/imu.h $(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/imu.c -o $@
@ -122,6 +119,21 @@ flight_imu_unittest : $(OBJECT_DIR)/flight/imu.o $(OBJECT_DIR)/flight/altitudeho
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/altitudehold.o : $(USER_DIR)/flight/altitudehold.c $(USER_DIR)/flight/altitudehold.h $(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/altitudehold.c -o $@
$(OBJECT_DIR)/altitude_hold_unittest.o : $(TEST_DIR)/altitude_hold_unittest.cc \
$(USER_DIR)/flight/altitudehold.h $(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/altitude_hold_unittest.cc -o $@
altitude_hold_unittest : $(OBJECT_DIR)/flight/altitudehold.o $(OBJECT_DIR)/altitude_hold_unittest.o $(OBJECT_DIR)/gtest_main.a
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/gps_conversion.o : $(USER_DIR)/flight/gps_conversion.c $(USER_DIR)/flight/gps_conversion.h $(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@

145
src/test/unit/altitude_hold_unittest.cc Normal file
View File

@ -0,0 +1,145 @@
/*
* 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 BARO
extern "C" {
#include "common/axis.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "drivers/accgyro.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "flight/mixer.h"
#include "flight/mixer.h"
#include "io/escservo.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
#include "flight/altitudehold.h"
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
#define DOWNWARDS_THRUST true
#define UPWARDS_THRUST false
extern "C" {
bool isThrustFacingDownwards(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];
printf("iteration: %d\n", index);
bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination);
EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result);
}
}
// STUBS
extern "C" {
uint32_t rcModeActivationMask;
int16_t rcCommand[4];
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[4];
uint8_t stateFlags;
uint16_t flightModeFlags;
uint8_t armingFlags;
int32_t sonarAlt;
void gyroGetADC(void) {};
bool sensors(uint32_t mask)
{
UNUSED(mask);
return false;
};
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
UNUSED(rollAndPitchTrims);
}
void accSum_reset(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;
}
}

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

@ -25,7 +25,6 @@
extern "C" {
#include "common/axis.h"
#include "flight/flight.h"
#include "flight/altitudehold.h"
#include "sensors/sensors.h"
#include "drivers/accgyro.h"
@ -47,47 +46,17 @@ extern "C" {
#define UPWARDS_THRUST false
extern "C" {
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations);
}
typedef struct inclinationExpectation_s {
rollAndPitchInclination_t inclination;
bool expectDownwardsThrust;
} inclinationExpectation_t;
TEST(FlightImuTest, IsThrustFacingDownwards)
TEST(FlightImuTest, Placeholder)
{
// 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];
printf("iteration: %d\n", index);
bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination);
EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result);
}
// TODO test things
EXPECT_EQ(true, true);
}
// STUBS
extern "C" {
uint32_t rcModeActivationMask;
int16_t rcCommand[4];
uint16_t acc_1G;
int16_t heading;
@ -114,6 +83,8 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
UNUSED(rollAndPitchTrims);
}
int32_t applyDeadband(int32_t, int32_t) { return 0; }
uint32_t micros(void) { return 0; }
bool isBaroCalibrationComplete(void) { return true; }
void performBaroCalibrationCycle(void) {}