src/test/unit/flight_mixer_unittest.cc.txt

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdint.h>
  19 #include <stdbool.h>
  20
  21 #include <limits.h>
  22
  23 extern "C" {
  24     #include "debug.h"
  25
  26     #include "platform.h"
  27
  28     #include "common/axis.h"
  29     #include "common/maths.h"
  30
  31     #include "drivers/sensor.h"
  32     #include "drivers/accgyro.h"
  33     #include "drivers/pwm_mapping.h"
  34
  35     #include "sensors/sensors.h"
  36     #include "sensors/acceleration.h"
  37
  38     #include "rx/rx.h"
  39     #include "flight/pid.h"
  40     #include "flight/imu.h"
  41     #include "flight/mixer.h"
  42     #include "flight/lowpass.h"
  43
  44     #include "io/escservo.h"
  45     #include "io/rc_controls.h"
  46
  47     void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers, servoMixer_t *initialCustomServoMixers);
  48     void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
  49 }
  50
  51 #include "unittest_macros.h"
  52 #include "gtest/gtest.h"
  53
  54 // input
  55 #define TEST_RC_MID 1500
  56
  57 // output
  58 #define TEST_MIN_COMMAND 1000
  59 #define TEST_SERVO_MID 1500
  60
  61 typedef struct motor_s {
  62     uint16_t value;
  63 } motor_t;
  64
  65 typedef struct servo_s {
  66     uint16_t value;
  67 } servo_t;
  68
  69 motor_t motors[MAX_SUPPORTED_MOTORS];
  70 servo_t servos[MAX_SUPPORTED_SERVOS];
  71
  72 uint8_t lastOneShotUpdateMotorCount;
  73
  74 uint32_t testFeatureMask = 0;
  75
  76 int updatedServoCount;
  77 int updatedMotorCount;
  78
  79 class ChannelForwardingTest : public ::testing::Test {
  80 protected:
  81     virtual void SetUp() {
  82         memset(&servos, 0, sizeof(servos));
  83     }
  84 };
  85
  86
  87 TEST_F(ChannelForwardingTest, TestForwardAuxChannelsToServosWithNoServos)
  88 {
  89
  90     rcData[AUX1] = TEST_RC_MID;
  91     rcData[AUX2] = TEST_RC_MID;
  92     rcData[AUX3] = TEST_RC_MID;
  93     rcData[AUX4] = TEST_RC_MID;
  94
  95     // when
  96
  97     // then
  98     for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
  99         EXPECT_EQ(0, servos[i].value);
 100     }
 101 }
 102
 103 TEST_F(ChannelForwardingTest, TestForwardAuxChannelsToServosWithMaxServos)
 104 {
 105     // given
 106
 107     rcData[AUX1] = 1000;
 108     rcData[AUX2] = 1250;
 109     rcData[AUX3] = 1750;
 110     rcData[AUX4] = 2000;
 111
 112     // when
 113     forwardAuxChannelsToServos(MAX_SUPPORTED_SERVOS);
 114
 115     // then
 116     uint8_t i;
 117     for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 118         EXPECT_EQ(0, servos[i].value);
 119     }
 120 }
 121
 122 TEST_F(ChannelForwardingTest, TestForwardAuxChannelsToServosWithLessRemainingServosThanAuxChannelsToForward)
 123 {
 124     // given
 125
 126     rcData[AUX1] = 1000;
 127     rcData[AUX2] = 1250;
 128     rcData[AUX3] = 1750;
 129     rcData[AUX4] = 2000;
 130
 131     // when
 132     forwardAuxChannelsToServos(MAX_SUPPORTED_SERVOS - 2);
 133
 134     // then
 135     uint8_t i;
 136     for (i = 0; i < MAX_SUPPORTED_SERVOS - 2; i++) {
 137         EXPECT_EQ(0, servos[i].value);
 138     }
 139
 140     // -1 for zero based offset
 141     EXPECT_EQ(1000, servos[MAX_SUPPORTED_SERVOS - 1 - 1].value);
 142     EXPECT_EQ(1250, servos[MAX_SUPPORTED_SERVOS - 0 - 1].value);
 143 }
 144
 145 class BasicMixerIntegrationTest : public ::testing::Test {
 146 protected:
 147     mixerConfig_t mixerConfig;
 148     rxConfig_t rxConfig;
 149     escAndServoConfig_t escAndServoConfig;
 150     servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
 151     gimbalConfig_t gimbalConfig = {
 152         .mode = GIMBAL_MODE_NORMAL
 153     };
 154
 155     motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
 156     servoMixer_t customServoMixer[MAX_SUPPORTED_SERVOS];
 157
 158     virtual void SetUp() {
 159         updatedServoCount = 0;
 160         updatedMotorCount = 0;
 161
 162         memset(&mixerConfig, 0, sizeof(mixerConfig));
 163         memset(&rxConfig, 0, sizeof(rxConfig));
 164         memset(&escAndServoConfig, 0, sizeof(escAndServoConfig));
 165         memset(&servoConf, 0, sizeof(servoConf));
 166
 167         memset(rcData, 0, sizeof(rcData));
 168         memset(rcCommand, 0, sizeof(rcCommand));
 169         memset(axisPID, 0, sizeof(axisPID));
 170
 171         memset(&customMotorMixer, 0, sizeof(customMotorMixer));
 172     }
 173
 174     virtual void withDefaultEscAndServoConfiguration(void) {
 175         escAndServoConfig.mincommand = TEST_MIN_COMMAND;
 176     }
 177
 178     virtual void withDefaultRxConfig(void) {
 179         rxConfig.midrc = 1500;
 180     }
 181
 182     virtual void configureMixer(void) {
 183         mixerUseConfigs(
 184             servoConf,
 185             &gimbalConfig,
 186             NULL,
 187             &escAndServoConfig,
 188             &mixerConfig,
 189             NULL,
 190             &rxConfig
 191         );
 192     }
 193 };
 194
 195 TEST_F(BasicMixerIntegrationTest, TestTricopterServo)
 196 {
 197     // given
 198     rxConfig.midrc = 1500;
 199
 200     mixerConfig.tri_unarmed_servo = 1;
 201
 202     withDefaultEscAndServoConfiguration();
 203     withDefaultRxConfig();
 204
 205     servoConf[5].min = DEFAULT_SERVO_MIN;
 206     servoConf[5].max = DEFAULT_SERVO_MAX;
 207     servoConf[5].middle = DEFAULT_SERVO_MIDDLE;
 208     servoConf[5].rate = 100;
 209
 210     configureMixer();
 211
 212     mixerInit(MIXER_TRI, customMotorMixer, customServoMixer);
 213
 214     // and
 215     pwmOutputConfiguration_t pwmOutputConfiguration = {
 216             .servoCount = 1,
 217             .motorCount = 3
 218     };
 219
 220     mixerUsePWMOutputConfiguration(&pwmOutputConfiguration);
 221
 222     // and
 223     axisPID[YAW] = 0;
 224
 225     // when
 226     mixTable();
 227     writeServos();
 228
 229     // then
 230     EXPECT_EQ(1, updatedServoCount);
 231     EXPECT_EQ(TEST_SERVO_MID, servos[0].value);
 232 }
 233
 234 TEST_F(BasicMixerIntegrationTest, TestQuadMotors)
 235 {
 236     // given
 237     withDefaultEscAndServoConfiguration();
 238
 239     configureMixer();
 240
 241     mixerInit(MIXER_QUADX, customMotorMixer, customServoMixer);
 242
 243     // and
 244     pwmOutputConfiguration_t pwmOutputConfiguration = {
 245             .servoCount = 0,
 246             .motorCount = 4
 247     };
 248
 249     mixerUsePWMOutputConfiguration(&pwmOutputConfiguration);
 250
 251     // and
 252     memset(rcCommand, 0, sizeof(rcCommand));
 253
 254     // and
 255     memset(axisPID, 0, sizeof(axisPID));
 256     axisPID[YAW] = 0;
 257
 258
 259     // when
 260     mixTable();
 261     writeMotors();
 262
 263     // then
 264     EXPECT_EQ(4, updatedMotorCount);
 265
 266     EXPECT_EQ(TEST_MIN_COMMAND, motors[0].value);
 267     EXPECT_EQ(TEST_MIN_COMMAND, motors[1].value);
 268     EXPECT_EQ(TEST_MIN_COMMAND, motors[2].value);
 269     EXPECT_EQ(TEST_MIN_COMMAND, motors[3].value);
 270 }
 271
 272
 273 class CustomMixerIntegrationTest : public BasicMixerIntegrationTest {
 274 protected:
 275
 276     virtual void SetUp() {
 277
 278         BasicMixerIntegrationTest::SetUp();
 279
 280         memset(&servoConf, 0, sizeof(servoConf));
 281         for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 282             servoConf[i].min = DEFAULT_SERVO_MIN;
 283             servoConf[i].max = DEFAULT_SERVO_MAX;
 284             servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
 285             servoConf[i].rate = 100;
 286         }
 287
 288         withDefaultEscAndServoConfiguration();
 289         withDefaultRxConfig();
 290
 291         configureMixer();
 292
 293         memset(&customMotorMixer, 0, sizeof(customMotorMixer));
 294         memset(&customServoMixer, 0, sizeof(customServoMixer));
 295     }
 296 };
 297
 298
 299 TEST_F(CustomMixerIntegrationTest, TestCustomMixer)
 300 {
 301     // given
 302     enum {
 303         EXPECTED_SERVOS_TO_MIX_COUNT = 6,
 304         EXPECTED_MOTORS_TO_MIX_COUNT = 2
 305     };
 306
 307     servoMixer_t testServoMixer[EXPECTED_SERVOS_TO_MIX_COUNT] = {
 308         { SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
 309         { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
 310         { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
 311         { SERVO_RUDDER, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
 312         { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
 313         { SERVO_FLAPS, INPUT_RC_AUX1,  100, 0, 0, 100, 0 },
 314     };
 315     memcpy(customServoMixer, testServoMixer, sizeof(testServoMixer));
 316
 317     static const motorMixer_t testMotorMixer[EXPECTED_MOTORS_TO_MIX_COUNT] = {
 318         { 1.0f,  0.0f,  0.0f, -1.0f },          // LEFT
 319         { 1.0f,  0.0f,  0.0f,  1.0f },          // RIGHT
 320     };
 321     memcpy(customMotorMixer, testMotorMixer, sizeof(testMotorMixer));
 322
 323     mixerInit(MIXER_CUSTOM_AIRPLANE, customMotorMixer, customServoMixer);
 324
 325     pwmOutputConfiguration_t pwmOutputConfiguration = {
 326             .servoCount = 6,
 327             .motorCount = 2
 328     };
 329
 330     mixerUsePWMOutputConfiguration(&pwmOutputConfiguration);
 331
 332     // and
 333     rcCommand[THROTTLE] = 1000;
 334
 335     // and
 336     rcData[AUX1] = 2000;
 337
 338     // and
 339     memset(axisPID, 0, sizeof(axisPID));
 340     axisPID[YAW] = 0;
 341
 342
 343     // when
 344     mixTable();
 345     writeMotors();
 346     writeServos();
 347
 348     // then
 349     EXPECT_EQ(EXPECTED_MOTORS_TO_MIX_COUNT, updatedMotorCount);
 350
 351     EXPECT_EQ(TEST_MIN_COMMAND, motors[0].value);
 352     EXPECT_EQ(TEST_MIN_COMMAND, motors[1].value);
 353
 354     EXPECT_EQ(EXPECTED_SERVOS_TO_MIX_COUNT, updatedServoCount);
 355
 356     EXPECT_EQ(TEST_SERVO_MID, servos[0].value);
 357     EXPECT_EQ(TEST_SERVO_MID, servos[1].value);
 358     EXPECT_EQ(TEST_SERVO_MID, servos[2].value);
 359     EXPECT_EQ(TEST_SERVO_MID, servos[3].value);
 360     EXPECT_EQ(1000, servos[4].value); // Throttle
 361     EXPECT_EQ(2000, servos[5].value); // Flaps
 362
 363 }
 364
 365 // STUBS
 366
 367 extern "C" {
 368 attitudeEulerAngles_t attitude;
 369 rxRuntimeConfig_t rxRuntimeConfig;
 370
 371 int16_t axisPID[XYZ_AXIS_COUNT];
 372 int16_t rcCommand[4];
 373 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 374
 375 uint32_t rcModeActivationMask;
 376 int32_t debug[DEBUG32_VALUE_COUNT];
 377
 378 uint8_t stateFlags;
 379 uint16_t flightModeFlags;
 380 uint8_t armingFlags;
 381
 382 void delay(uint32_t) {}
 383
 384 bool feature(uint32_t mask) {
 385     return (mask & testFeatureMask);
 386 }
 387
 388 int32_t lowpassFixed(lowpass_t *, int32_t, int16_t) {
 389     return 0;
 390 }
 391
 392 void pwmWriteMotor(uint8_t index, uint16_t value) {
 393     motors[index].value = value;
 394     updatedMotorCount++;
 395 }
 396
 397 void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
 398 {
 399     uint8_t index;
 400
 401     for(index = 0; index < motorCount; index++){
 402         motors[index].value = 0;
 403     }
 404 }
 405
 406 void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) {
 407     lastOneShotUpdateMotorCount = motorCount;
 408 }
 409
 410 void pwmWriteServo(uint8_t index, uint16_t value) {
 411     // FIXME logic in test, mimic's production code.
 412     // Perhaps the solution is to remove the logic from the production code version and assume that
 413     // anything calling calling pwmWriteServo always uses a valid index?
 414     // See MAX_SERVOS in pwm_output (driver) and MAX_SUPPORTED_SERVOS (flight)
 415     if (index < MAX_SERVOS) {
 416         servos[index].value = value;
 417     }
 418     updatedServoCount++;
 419 }
 420
 421 bool failsafeIsActive(void) {
 422     return false;
 423 }
 424
 425 }