diff --git a/Makefile b/Makefile index 2b47c99674..bb8cdfbd50 100644 --- a/Makefile +++ b/Makefile @@ -199,6 +199,7 @@ COMMON_SRC = build_config.c \ common/maths.c \ common/printf.c \ common/typeconversion.c \ + common/encoding.c \ main.c \ mw.c \ flight/altitudehold.c \ diff --git a/docs/Cli.md b/docs/Cli.md index 93b5bf1820..f07eedbf9f 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -201,7 +201,7 @@ Re-apply any new defaults as desired. | failsafe_min_usec | | 100 | 2000 | 985 | Profile | UINT16 | | failsafe_max_usec | | 100 | 3000 | 2115 | Profile | UINT16 | | gimbal_flags | When feature SERVO_TILT is enabled, this can be a combination of the following numbers: 1=normal gimbal (default), 2=tiltmix gimbal, 4=in PPM (or SERIALRX) input mode, this will forward AUX1..4 RC inputs to PWM5..8 pins | 0 | 255 | 1 | Profile | UINT8 | -| acc_hardware | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 1 for ADXL345, 2 for MPU6050 integrated accelerometer, 3 for MMA8452, 4 for BMA280, or 5 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 | +| acc_hardware | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for SPI_MPU6000, 8 for SPI_MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 | | acc_lpf_factor | | 0 | 250 | 4 | Profile | UINT8 | | accxy_deadband | | 0 | 100 | 40 | Profile | UINT8 | | accz_deadband | | 0 | 100 | 40 | Profile | UINT8 | diff --git a/docs/LedStrip.md b/docs/LedStrip.md index 49394b65ad..df0373ea8f 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -406,7 +406,7 @@ led 27 2,9:S:FWT:0 ``` 16-18 9-11 19-21 \ / 6-8 - \ 13-16 / + \ 12-15 / \ FRONT / / BACK \ / \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 0461dac74a..ae94762975 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -26,6 +26,7 @@ #include "common/maths.h" #include "common/axis.h" #include "common/color.h" +#include "common/encoding.h" #include "drivers/gpio.h" #include "drivers/sensor.h" @@ -878,11 +879,6 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he */ static bool blackboxWriteSysinfo() { - union floatConvert_t { - float f; - uint32_t u; - } floatConvert; - if (xmitState.headerIndex == 0) { xmitState.u.serialBudget = 0; xmitState.headerIndex = 1; @@ -937,8 +933,7 @@ static bool blackboxWriteSysinfo() xmitState.u.serialBudget -= strlen("H maxthrottle:%d\n"); break; case 8: - floatConvert.f = gyro.scale; - blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u); + blackboxPrintf("H gyro.scale:0x%x\n", castFloatBytesToInt(gyro.scale)); xmitState.u.serialBudget -= strlen("H gyro.scale:0x\n") + 6; break; diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index fa6f66d6d6..ac8bf5ef49 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -4,12 +4,13 @@ #include "blackbox_io.h" -#include "platform.h" #include "version.h" +#include "build_config.h" #include "common/maths.h" #include "common/axis.h" #include "common/color.h" +#include "common/encoding.h" #include "drivers/gpio.h" #include "drivers/sensor.h" @@ -144,18 +145,6 @@ void blackboxWriteUnsignedVB(uint32_t value) blackboxWrite(value); } -/** - * ZigZag encoding maps all values of a signed integer into those of an unsigned integer in such - * a way that numbers of small absolute value correspond to small integers in the result. - * - * (Compared to just casting a signed to an unsigned which creates huge resulting numbers for - * small negative integers). - */ -static uint32_t zigzagEncode(int32_t value) -{ - return (uint32_t)((value << 1) ^ (value >> 31)); -} - /** * Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding. */ diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 62d5909986..547850739c 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -20,7 +20,7 @@ #include #include -#include "target.h" +#include "platform.h" typedef enum BlackboxDevice { BLACKBOX_DEVICE_SERIAL = 0, diff --git a/src/main/common/encoding.c b/src/main/common/encoding.c new file mode 100644 index 0000000000..3823e862a7 --- /dev/null +++ b/src/main/common/encoding.c @@ -0,0 +1,31 @@ +#include "encoding.h" + +/** + * Cast the in-memory representation of the given float directly to an int. + * + * This is useful for printing the hex representation of a float number (which is considerably cheaper + * than a full decimal float formatter, in both code size and output length). + */ +uint32_t castFloatBytesToInt(float f) +{ + union floatConvert_t { + float f; + uint32_t u; + } floatConvert; + + floatConvert.f = f; + + return floatConvert.u; +} + +/** + * ZigZag encoding maps all values of a signed integer into those of an unsigned integer in such + * a way that numbers of small absolute value correspond to small integers in the result. + * + * (Compared to just casting a signed to an unsigned which creates huge resulting numbers for + * small negative integers). + */ +uint32_t zigzagEncode(int32_t value) +{ + return (uint32_t)((value << 1) ^ (value >> 31)); +} diff --git a/src/main/common/encoding.h b/src/main/common/encoding.h new file mode 100644 index 0000000000..e0681ed2a6 --- /dev/null +++ b/src/main/common/encoding.h @@ -0,0 +1,23 @@ +/* + * 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 . + */ + +#pragma once + +#include + +uint32_t castFloatBytesToInt(float f); +uint32_t zigzagEncode(int32_t value); diff --git a/src/main/config/config.c b/src/main/config/config.c index b489ff3bd7..a9adeb3f67 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -119,7 +119,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 92; +static const uint8_t EEPROM_CONF_VERSION = 93; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -620,6 +620,8 @@ void activateConfig(void) activateControlRateConfig(); + resetAdjustmentStates(); + useRcControlsConfig( currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d247b5bed6..ff779782ed 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -26,6 +26,7 @@ #include "common/axis.h" #include "common/maths.h" +#include "drivers/system.h" #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_output.h" @@ -487,6 +488,13 @@ void writeAllMotors(int16_t mc) writeMotors(); } +void stopMotors(void) +{ + writeAllMotors(escAndServoConfig->mincommand); + + delay(50); // give the timers and ESCs a chance to react. +} + #ifndef USE_QUAD_MIXER_ONLY static void airplaneMixer(void) { @@ -537,7 +545,6 @@ static void airplaneMixer(void) void mixTable(void) { - int16_t maxMotor; uint32_t i; if (motorCount > 3) { @@ -655,29 +662,40 @@ void mixTable(void) } #endif - maxMotor = motor[0]; - for (i = 1; i < motorCount; i++) - if (motor[i] > maxMotor) - maxMotor = motor[i]; - for (i = 0; i < motorCount; i++) { - if (maxMotor > escAndServoConfig->maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max. - motor[i] -= maxMotor - escAndServoConfig->maxthrottle; - if (feature(FEATURE_3D)) { - if ((rcData[THROTTLE]) > rxConfig->midrc) { - motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); - } else { - motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low); - } - } else { - motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); - if ((rcData[THROTTLE]) < rxConfig->mincheck) { - if (!feature(FEATURE_MOTOR_STOP)) - motor[i] = escAndServoConfig->minthrottle; - else - motor[i] = escAndServoConfig->mincommand; + if (ARMING_FLAG(ARMED)) { + + int16_t maxMotor = motor[0]; + for (i = 1; i < motorCount; i++) { + if (motor[i] > maxMotor) { + maxMotor = motor[i]; } } - if (!ARMING_FLAG(ARMED)) { + + for (i = 0; i < motorCount; i++) { + if (maxMotor > escAndServoConfig->maxthrottle) { + // this is a way to still have good gyro corrections if at least one motor reaches its max. + motor[i] -= maxMotor - escAndServoConfig->maxthrottle; + } + + if (feature(FEATURE_3D)) { + if ((rcData[THROTTLE]) > rxConfig->midrc) { + motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); + } else { + motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low); + } + } else { + motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); + if ((rcData[THROTTLE]) < rxConfig->mincheck) { + if (!feature(FEATURE_MOTOR_STOP)) + motor[i] = escAndServoConfig->minthrottle; + else + motor[i] = escAndServoConfig->mincommand; + } + } + } + + } else { + for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 38bba6b741..627199af3b 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -109,3 +109,4 @@ void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerResetMotors(void); void mixTable(void); void writeMotors(void); +void stopMotors(void); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 01ade6bc80..e14683dc0e 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -593,3 +593,9 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es } } } + +void resetAdjustmentStates(void) +{ + memset(adjustmentStates, 0, sizeof(adjustmentStates)); +} + diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index dde0536f06..a1549f5c5f 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -214,6 +214,7 @@ typedef struct adjustmentState_s { #define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches. +void resetAdjustmentStates(void); void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 29c2f5497c..de20b073df 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -161,13 +161,11 @@ static const char * const sensorTypeNames[] = { "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL }; -// FIXME the next time the EEPROM is bumped change the order of acc and gyro names so that "None" is second. - #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) static const char * const sensorHardwareNames[4][11] = { { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL }, - { "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL }, + { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", NULL }, { "", "None", "BMP085", "MS5611", NULL }, { "", "None", "HMC5883", "AK8975", NULL } }; @@ -1344,6 +1342,7 @@ static void cliRateProfile(char *cmdline) static void cliReboot(void) { cliPrint("\r\nRebooting"); waitForSerialPortToFinishTransmitting(cliPort); + stopMotors(); systemReset(); } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 942c009a80..408bea6539 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1688,6 +1688,7 @@ void mspProcess(void) while (!isSerialTransmitBufferEmpty(candidatePort->port)) { delay(50); } + stopMotors(); systemReset(); } } diff --git a/src/main/main.c b/src/main/main.c index 6e703d0f30..37224c18be 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -127,11 +127,20 @@ void SetSysClock(void); void SetSysClock(bool overclock); #endif +typedef enum { + SYSTEM_STATE_INITIALISING = 0, + SYSTEM_STATE_CONFIG_LOADED = (1 << 0), + SYSTEM_STATE_SENSORS_READY = (1 << 1), + SYSTEM_STATE_MOTORS_READY = (1 << 2), + SYSTEM_STATE_READY = (1 << 7) +} systemState_e; + +static uint8_t systemState = SYSTEM_STATE_INITIALISING; + void init(void) { uint8_t i; drv_pwm_config_t pwm_params; - bool sensorsOK = false; printfSupportInit(); @@ -140,6 +149,8 @@ void init(void) ensureEEPROMContainsValidData(); readEEPROM(); + systemState |= SYSTEM_STATE_CONFIG_LOADED; + #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); @@ -222,6 +233,8 @@ void init(void) mixerUsePWMOutputConfiguration(pwmOutputConfiguration); + systemState |= SYSTEM_STATE_MOTORS_READY; + #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPin = BEEP_PIN, @@ -300,11 +313,12 @@ void init(void) } #endif - sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination); - - // if gyro was not detected due to whatever reason, we give up now. - if (!sensorsOK) + if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination)) { + // if gyro was not detected due to whatever reason, we give up now. failureMode(3); + } + + systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; @@ -329,7 +343,9 @@ void init(void) serialInit(&masterConfig.serialConfig); failsafe = failsafeInit(&masterConfig.rxConfig); + beepcodeInit(failsafe); + rxInit(&masterConfig.rxConfig, failsafe); #ifdef GPS @@ -425,6 +441,8 @@ void init(void) #ifdef CJMCU LED2_ON; #endif + + systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK @@ -453,6 +471,9 @@ int main(void) { void HardFault_Handler(void) { // fall out of the sky - writeAllMotors(masterConfig.escAndServoConfig.mincommand); + uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; + if ((systemState & requiredState) == requiredState) { + stopMotors(); + } while (1); } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 5b925b01d4..198b12778a 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -20,15 +20,15 @@ // Type of accelerometer used/detected typedef enum { ACC_DEFAULT = 0, - ACC_ADXL345 = 1, - ACC_MPU6050 = 2, - ACC_MMA8452 = 3, - ACC_BMA280 = 4, - ACC_LSM303DLHC = 5, - ACC_SPI_MPU6000 = 6, - ACC_SPI_MPU6500 = 7, - ACC_FAKE = 8, - ACC_NONE = 9 + ACC_NONE = 1, + ACC_ADXL345 = 2, + ACC_MPU6050 = 3, + ACC_MMA8452 = 4, + ACC_BMA280 = 5, + ACC_LSM303DLHC = 6, + ACC_SPI_MPU6000 = 7, + ACC_SPI_MPU6500 = 8, + ACC_FAKE = 9, } accelerationSensor_e; extern sensor_align_e accAlign; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index cd90c28f36..d3bf32a0ef 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -110,7 +110,7 @@ const mpu6050Config_t *selectMPU6050Config(void) #ifdef USE_FAKE_GYRO static void fakeGyroInit(void) {} static void fakeGyroRead(int16_t *gyroData) { - UNUSED(gyroData); + memset(gyroData, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); } static void fakeGyroReadTemp(int16_t *tempData) { UNUSED(tempData); @@ -129,7 +129,7 @@ bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf) #ifdef USE_FAKE_ACC static void fakeAccInit(void) {} static void fakeAccRead(int16_t *accData) { - UNUSED(accData); + memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); } bool fakeAccDetect(acc_t *acc) @@ -267,14 +267,6 @@ retry: switch (accHardwareToUse) { case ACC_DEFAULT: ; // fallthrough - case ACC_FAKE: -#ifdef USE_FAKE_ACC - if (fakeAccDetect(&acc)) { - accHardware = ACC_FAKE; - break; - } -#endif - ; // fallthrough case ACC_ADXL345: // ADXL345 #ifdef USE_ACC_ADXL345 acc_params.useFifo = false; @@ -365,6 +357,14 @@ retry: accHardware = ACC_SPI_MPU6500; break; } +#endif + ; // fallthrough + case ACC_FAKE: +#ifdef USE_FAKE_ACC + if (fakeAccDetect(&acc)) { + accHardware = ACC_FAKE; + break; + } #endif ; // fallthrough case ACC_NONE: // disable ACC diff --git a/src/test/Makefile b/src/test/Makefile index 5ea38fe9f0..6575c48a1a 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -53,6 +53,7 @@ TESTS = \ rc_controls_unittest \ ledstrip_unittest \ ws2811_unittest \ + encoding_unittest \ lowpass_unittest # All Google Test headers. Usually you shouldn't change this @@ -130,6 +131,25 @@ battery_unittest : \ $(OBJECT_DIR)/gtest_main.a $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + +$(OBJECT_DIR)/common/encoding.o : $(USER_DIR)/common/encoding.c $(USER_DIR)/common/encoding.h $(GTEST_HEADERS) + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/encoding.c -o $@ + +$(OBJECT_DIR)/encoding_unittest.o : \ + $(TEST_DIR)/encoding_unittest.cc \ + $(USER_DIR)/common/encoding.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/encoding_unittest.cc -o $@ + +encoding_unittest : \ + $(OBJECT_DIR)/common/encoding.o \ + $(OBJECT_DIR)/encoding_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ $(OBJECT_DIR)/flight/imu.o : \ $(USER_DIR)/flight/imu.c \ diff --git a/src/test/unit/encoding_unittest.cc b/src/test/unit/encoding_unittest.cc new file mode 100644 index 0000000000..bf2c70b916 --- /dev/null +++ b/src/test/unit/encoding_unittest.cc @@ -0,0 +1,84 @@ +/* + * 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 + +extern "C" { + #include "common/encoding.h" +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +typedef struct zigzagEncodingExpectation_t { + int32_t input; + uint32_t expected; +} zigzagEncodingExpectation_t; + +typedef struct floatToIntEncodingExpectation_t { + float input; + uint32_t expected; +} floatToIntEncodingExpectation_t; + +TEST(EncodingTest, ZigzagEncodingTest) +{ + // given + zigzagEncodingExpectation_t expectations[] = { + { 0, 0}, + {-1, 1}, + { 1, 2}, + {-2, 3}, + { 2, 4}, + + { 2147483646, 4294967292}, + {-2147483647, 4294967293}, + { 2147483647, 4294967294}, + {-2147483648, 4294967295}, + }; + int expectationCount = sizeof(expectations) / sizeof(expectations[0]); + + // expect + + for (int i = 0; i < expectationCount; i++) { + zigzagEncodingExpectation_t *expectation = &expectations[i]; + + EXPECT_EQ(expectation->expected, zigzagEncode(expectation->input)); + } +} + +TEST(EncodingTest, FloatToIntEncodingTest) +{ + // given + floatToIntEncodingExpectation_t expectations[] = { + {0.0, 0x00000000}, + {2.0, 0x40000000}, // Exponent should be in the top bits + {4.5, 0x40900000} + }; + int expectationCount = sizeof(expectations) / sizeof(expectations[0]); + + // expect + + for (int i = 0; i < expectationCount; i++) { + floatToIntEncodingExpectation_t *expectation = &expectations[i]; + + EXPECT_EQ(expectation->expected, castFloatBytesToInt(expectation->input)); + } +} + +// STUBS + +extern "C" { +}