From 44475985a2b8d80218e40583b6e0be60df9c4f2e Mon Sep 17 00:00:00 2001 From: Steveis Date: Tue, 23 Jun 2015 17:41:22 +0100 Subject: [PATCH 1/2] MS5611 CRC Unit test --- src/main/drivers/barometer_ms5611.c | 6 +- src/test/Makefile | 26 +++++++- src/test/unit/baro_unittest.cc | 99 +++++++++++++++++++++++++++++ 3 files changed, 128 insertions(+), 3 deletions(-) create mode 100644 src/test/unit/baro_unittest.cc diff --git a/src/main/drivers/barometer_ms5611.c b/src/main/drivers/barometer_ms5611.c index 7ff5b3f0f3..897983db75 100644 --- a/src/main/drivers/barometer_ms5611.c +++ b/src/main/drivers/barometer_ms5611.c @@ -26,6 +26,8 @@ #include "system.h" #include "bus_i2c.h" +#include "build_config.h" + // MS5611, Standard address 0x77 #define MS5611_ADDR 0x77 @@ -44,7 +46,7 @@ static void ms5611_reset(void); static uint16_t ms5611_prom(int8_t coef_num); -static int8_t ms5611_crc(uint16_t *prom); +STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom); static uint32_t ms5611_read_adc(void); static void ms5611_start_ut(void); static void ms5611_get_ut(void); @@ -102,7 +104,7 @@ static uint16_t ms5611_prom(int8_t coef_num) return rxbuf[0] << 8 | rxbuf[1]; } -static int8_t ms5611_crc(uint16_t *prom) +STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom) { int32_t i, j; uint32_t res = 0; diff --git a/src/test/Makefile b/src/test/Makefile index f46dc69fc6..b573acf77e 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -58,7 +58,8 @@ TESTS = \ ws2811_unittest \ encoding_unittest \ io_serial_unittest \ - lowpass_unittest + lowpass_unittest \ + baro_unittest # All Google Test headers. Usually you shouldn't change this # definition. @@ -463,6 +464,29 @@ rx_rx_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/drivers/barometer_ms5611.o : \ + $(USER_DIR)/drivers/barometer_ms5611.c \ + $(USER_DIR)/drivers/barometer_ms5611.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/barometer_ms5611.c -o $@ + +$(OBJECT_DIR)/baro_unittest.o : \ + $(TEST_DIR)/baro_unittest.cc \ + $(USER_DIR)/drivers/barometer_ms5611.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/baro_unittest.cc -o $@ + +baro_unittest : \ + $(OBJECT_DIR)/drivers/barometer_ms5611.o \ + $(OBJECT_DIR)/baro_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ + test: $(TESTS) set -e && for test in $(TESTS) ; do \ $(OBJECT_DIR)/$$test; \ diff --git a/src/test/unit/baro_unittest.cc b/src/test/unit/baro_unittest.cc new file mode 100644 index 0000000000..b84052c271 --- /dev/null +++ b/src/test/unit/baro_unittest.cc @@ -0,0 +1,99 @@ +/* + * 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" { + +int8_t ms5611_crc(uint16_t *prom); + +} + + +#include "unittest_macros.h" +#include "gtest/gtest.h" + + +TEST(baroTest, TestValidMs5611Crc) +{ + + // given + uint16_t ms5611_prom[] = {0x3132,0x3334,0x3536,0x3738,0x3940,0x4142,0x4344,0x450B}; + + // when + int8_t result = ms5611_crc(ms5611_prom); + + // then + EXPECT_EQ(0, result); + +} + +TEST(baroTest, TestInvalidMs5611Crc) +{ + + // given + uint16_t ms5611_prom[] = {0x3132,0x3334,0x3536,0x3738,0x3940,0x4142,0x4344,0x4500}; + + // when + int8_t result = ms5611_crc(ms5611_prom); + + // then + EXPECT_EQ(-1, result); + +} + +TEST(baroTest, TestMs5611AllZeroProm) +{ + + // given + uint16_t ms5611_prom[] = {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}; + + // when + int8_t result = ms5611_crc(ms5611_prom); + + // then + EXPECT_EQ(-1, result); + +} + +TEST(baroTest, TestMs5611AllOnesProm) +{ + + // given + uint16_t ms5611_prom[] = {0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF}; + + // when + int8_t result = ms5611_crc(ms5611_prom); + + // then + EXPECT_EQ(-1, result); + +} + +// STUBS + +extern "C" { + +void delay(uint32_t) {} +void delayMicroseconds(uint32_t) {} +bool i2cWrite(uint8_t, uint8_t, uint8_t) { + return 1; +} +bool i2cRead(uint8_t, uint8_t, uint8_t, uint8_t) { + return 1; +} + +} From 13d923a997a4367116651cf0d1ca18ce911b9068 Mon Sep 17 00:00:00 2001 From: "U-possi\\David" Date: Wed, 13 May 2015 00:18:34 +0200 Subject: [PATCH 2/2] Smartport telemetry fixes. Closes #818 and #877. --- src/main/telemetry/smartport.c | 20 ++++++++------------ 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index ca14df13e6..cf55e8ccc2 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -246,6 +246,7 @@ void configureSmartPortTelemetryPort(void) smartPortState = SPSTATE_INITIALIZED; smartPortTelemetryEnabled = true; + smartPortLastRequestTime = millis(); } bool canSendSmartPortTelemetry(void) @@ -348,19 +349,14 @@ void handleSmartPortTelemetry(void) // the MSB of the sent uint32_t helps FrSky keep track // the even/odd bit of our counter helps us keep track if (smartPortIdCnt & 1) { - tmpui = tmpi = GPS_coord[LON]; - if (tmpi < 0) { - tmpui = -tmpi; - tmpui |= 0x40000000; - } - tmpui |= 0x80000000; + tmpui = abs(GPS_coord[LON]); // now we have unsigned value and one bit to spare + tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast + if (GPS_coord[LON] < 0) tmpui |= 0x40000000; } else { - tmpui = tmpi = GPS_coord[LAT]; - if (tmpi < 0) { - tmpui = -tmpi; - tmpui |= 0x40000000; - } + tmpui = abs(GPS_coord[LAT]); // now we have unsigned value and one bit to spare + tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast + if (GPS_coord[LAT] < 0) tmpui |= 0x40000000; } smartPortSendPackage(id, tmpui); smartPortHasRequest = 0; @@ -452,7 +448,7 @@ void handleSmartPortTelemetry(void) #ifdef GPS case FSSP_DATAID_GPS_ALT : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - smartPortSendPackage(id, GPS_altitude * 1000); // given in 0.1m , requested in 100 = 1m + smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) smartPortHasRequest = 0; } break;