From 503e7a08172ad7fd4f0c33211e77a12ce50e5527 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 3 Nov 2014 20:03:21 +0100 Subject: [PATCH] Decouple altitudehold.c from config.c. Update flight_imu_unittest and altitude_hold_unittest. --- src/main/config/config.c | 17 ++- src/main/config/config_master.h | 1 - src/main/config/config_profile.h | 7 +- src/main/flight/altitudehold.c | 66 +++++------ src/main/flight/altitudehold.h | 9 +- src/main/flight/flight.h | 1 - src/main/flight/imu.c | 10 -- src/main/flight/imu.h | 4 - src/main/flight/mixer.h | 1 + src/main/io/rc_controls.h | 7 ++ src/main/io/serial_cli.c | 11 +- src/main/mw.c | 22 ++-- src/test/Makefile | 20 +++- src/test/unit/altitude_hold_unittest.cc | 145 ++++++++++++++++++++++++ src/test/unit/flight_imu_unittest.cc | 43 ++----- 15 files changed, 243 insertions(+), 121 deletions(-) create mode 100644 src/test/unit/altitude_hold_unittest.cc diff --git a/src/main/config/config.c b/src/main/config/config.c index 0ef033bd2..6915f5a4e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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(¤tProfile->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, ¤tProfile->pidProfile, ¤tProfile->accDeadband); - configureAltitudeHold(¤tProfile->pidProfile, ¤tProfile->barometerConfig); + configureAltitudeHold(¤tProfile->pidProfile, ¤tProfile->barometerConfig, ¤tProfile->rcControlsConfig, &masterConfig.escAndServoConfig); calculateThrottleAngleScale(currentProfile->throttle_correction_angle); calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 180f0399d..c354d9e58 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -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; diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 8062d5490..1aafc14bd 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -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. diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 6bb7f67ce..20ffbd45b 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -21,7 +21,6 @@ #include #include -#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(); } diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index f4dfee0a9..fefbb7582 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -15,9 +15,14 @@ * along with Cleanflight. If not, see . */ -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); diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index 068a2a304..a6fadf55f 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -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; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index f525d5166..b79562fa0 100755 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 6fc93375c..d8039f2b5 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ab4f9dea7..ad846dc33 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -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 diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index b40f01d8d..051e25214 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -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); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b556f96e4..7dee236aa 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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 }, diff --git a/src/main/mw.c b/src/main/mw.c index 45cf6aedb..bc56fb1e9 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -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 diff --git a/src/test/Makefile b/src/test/Makefile index 92ca4f1c1..4dd0cf1ab 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -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 $@ diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc new file mode 100644 index 000000000..b9df220b1 --- /dev/null +++ b/src/test/unit/altitude_hold_unittest.cc @@ -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 . + */ + +#include +#include + +#include + +#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; +} + +} diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 50c35918d..83ac36b33 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -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) {}