src/test/unit/flight_mixer_unittest.cc

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