From 5a28ce5129cedb5216d9a614fe7e30b1a6b47c48 Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Fri, 10 May 2024 05:23:32 +0200 Subject: [PATCH] Make Cppcheck happier revived (#13566) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Štěpán Dalecký --- lib/main/MAVLink/mavlink_helpers.h | 2 - src/main/cli/cli.c | 30 +++++------ src/main/cms/cms.c | 2 +- src/main/cms/cms.h | 2 +- src/main/common/maths.c | 18 +++---- src/main/common/maths.h | 18 +++---- src/main/common/sdft.c | 3 +- src/main/common/time.c | 6 +-- src/main/common/time.h | 6 +-- src/main/drivers/accgyro/accgyro_mpu.c | 4 +- src/main/drivers/adc.c | 2 +- src/main/drivers/adc.h | 2 +- src/main/drivers/adc_impl.h | 2 +- src/main/drivers/at32/timer_at32bsp.c | 4 +- src/main/drivers/at32/timer_at32f43x.c | 2 +- src/main/drivers/bus_spi.c | 2 +- src/main/drivers/bus_spi.h | 4 +- src/main/drivers/bus_spi_config.c | 2 +- src/main/drivers/compass/compass_ak8963.c | 2 +- src/main/drivers/compass/compass_ak8975.c | 2 +- src/main/drivers/flash_impl.h | 2 +- src/main/drivers/flash_m25p16.c | 4 +- src/main/drivers/flash_w25m.c | 2 +- src/main/drivers/flash_w25n01g.c | 4 +- src/main/drivers/flash_w25q128fv.c | 2 +- src/main/drivers/pwm_output_dshot_shared.c | 2 +- src/main/drivers/serial_tcp.c | 6 +-- src/main/drivers/stm32/bus_spi_ll.c | 2 +- src/main/drivers/stm32/bus_spi_stdperiph.c | 2 +- src/main/drivers/stm32/serial_uart_hal.c | 2 +- src/main/drivers/stm32/timer_hal.c | 4 +- src/main/drivers/stm32/timer_stdperiph.c | 4 +- src/main/drivers/stm32/timer_stm32f4xx.c | 2 +- src/main/drivers/stm32/timer_stm32f7xx.c | 2 +- src/main/drivers/stm32/timer_stm32g4xx.c | 2 +- src/main/drivers/stm32/timer_stm32h7xx.c | 2 +- src/main/drivers/timer.h | 6 +-- src/main/fc/rc_modes.c | 2 +- src/main/fc/rc_modes.h | 2 +- src/main/flight/imu.c | 2 +- src/main/flight/mixer.c | 2 +- src/main/flight/pid.c | 2 +- src/main/io/asyncfatfs/asyncfatfs.c | 4 +- src/main/io/gps.c | 61 ++++++++++++---------- src/main/io/gps.h | 2 +- src/main/io/ledstrip.c | 11 ++-- src/main/io/rcdevice.c | 2 +- src/main/io/serial.c | 2 +- src/main/io/serial_4way_avrootloader.c | 4 +- src/main/io/vtx_smartaudio.c | 2 +- src/main/io/vtx_tramp.c | 2 +- src/main/rx/cc2500_frsky_d.c | 2 +- src/main/rx/cc2500_frsky_shared.c | 2 +- src/main/rx/jetiexbus.c | 5 +- src/main/rx/jetiexbus.h | 2 +- src/main/rx/msp.c | 2 +- src/main/rx/msp.h | 2 +- src/main/scheduler/scheduler.c | 2 +- src/main/telemetry/crsf.c | 2 +- src/main/telemetry/hott.c | 3 +- src/main/telemetry/ibus_shared.c | 9 ++-- src/main/telemetry/jetiexbus.c | 4 +- src/test/unit/scheduler_unittest.cc | 2 +- 63 files changed, 145 insertions(+), 157 deletions(-) diff --git a/lib/main/MAVLink/mavlink_helpers.h b/lib/main/MAVLink/mavlink_helpers.h index 7cfbc0be40..b0199c139e 100644 --- a/lib/main/MAVLink/mavlink_helpers.h +++ b/lib/main/MAVLink/mavlink_helpers.h @@ -289,7 +289,6 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status - int bufferIndex = 0; status->msg_received = 0; @@ -425,7 +424,6 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa break; } - bufferIndex++; // If a message has been sucessfully decoded, check index if (status->msg_received == 1) { diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index a4fc93549c..437e10865a 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -1636,28 +1636,26 @@ static void printAdjustmentRange(dumpFlags_t dumpMask, const adjustmentRange_t * static void cliAdjustmentRange(const char *cmdName, char *cmdline) { const char *format = "adjrange %u 0 %u %u %u %u %u %u %u"; - int i, val = 0; const char *ptr; if (isEmpty(cmdline)) { printAdjustmentRange(DUMP_MASTER, adjustmentRanges(0), NULL, NULL); } else { ptr = cmdline; - i = atoi(ptr++); + int i = atoi(ptr++); if (i < MAX_ADJUSTMENT_RANGE_COUNT) { adjustmentRange_t *ar = adjustmentRangesMutable(i); uint8_t validArgumentCount = 0; ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); // Was: slot // Keeping the parameter to retain backwards compatibility for the command format. validArgumentCount++; } ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { ar->auxChannelIndex = val; validArgumentCount++; @@ -1668,7 +1666,7 @@ static void cliAdjustmentRange(const char *cmdName, char *cmdline) ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); if (val >= 0 && val < ADJUSTMENT_FUNCTION_COUNT) { ar->adjustmentConfig = val; validArgumentCount++; @@ -1676,7 +1674,7 @@ static void cliAdjustmentRange(const char *cmdName, char *cmdline) } ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { ar->auxSwitchChannelIndex = val; validArgumentCount++; @@ -1695,13 +1693,13 @@ static void cliAdjustmentRange(const char *cmdName, char *cmdline) ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); ar->adjustmentCenter = val; validArgumentCount++; } ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); ar->adjustmentScale = val; validArgumentCount++; } @@ -2616,7 +2614,6 @@ static void printVtx(dumpFlags_t dumpMask, const vtxConfig_t *vtxConfig, const v static void cliVtx(const char *cmdName, char *cmdline) { const char *format = "vtx %u %u %u %u %u %u %u"; - int i, val = 0; const char *ptr; if (isEmpty(cmdline)) { @@ -2632,13 +2629,13 @@ static void cliVtx(const char *cmdName, char *cmdline) const uint8_t maxPowerIndex = VTX_TABLE_MAX_POWER_LEVELS; #endif ptr = cmdline; - i = atoi(ptr++); + int i = atoi(ptr++); if (i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT) { vtxChannelActivationCondition_t *cac = &vtxConfigMutable()->vtxChannelActivationConditions[i]; uint8_t validArgumentCount = 0; ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { cac->auxChannelIndex = val; validArgumentCount++; @@ -2646,7 +2643,7 @@ static void cliVtx(const char *cmdName, char *cmdline) } ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); if (val >= 0 && val <= maxBandIndex) { cac->band = val; validArgumentCount++; @@ -2654,7 +2651,7 @@ static void cliVtx(const char *cmdName, char *cmdline) } ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); if (val >= 0 && val <= maxChannelIndex) { cac->channel = val; validArgumentCount++; @@ -2662,7 +2659,7 @@ static void cliVtx(const char *cmdName, char *cmdline) } ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); if (val >= 0 && val <= maxPowerIndex) { cac->power= val; validArgumentCount++; @@ -4377,7 +4374,7 @@ STATIC_UNIT_TESTED void cliGet(const char *cmdName, char *cmdline) } } -static uint8_t getWordLength(char *bufBegin, char *bufEnd) +static uint8_t getWordLength(const char *bufBegin, const char *bufEnd) { while (*(bufEnd - 1) == ' ') { bufEnd--; @@ -5438,9 +5435,8 @@ static void printTimerDmaoptDetails(const ioTag_t ioTag, const timerHardware_t * if (printDetails) { const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByTimerValue(timer->tim, timer->channel, dmaopt); - dmaCode_t dmaCode = 0; if (dmaChannelSpec) { - dmaCode = dmaChannelSpec->code; + dmaCode_t dmaCode = dmaChannelSpec->code; printValue(dumpMask, false, "# pin %c%02d: " DMASPEC_FORMAT_STRING, IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag), diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 2f6241d5cd..c1a3ac711f 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -130,7 +130,7 @@ static displayPort_t *cmsDisplayPortSelectNext(void) return cmsDisplayPorts[cmsCurrentDevice]; } -bool cmsDisplayPortSelect(displayPort_t *instance) +bool cmsDisplayPortSelect(const displayPort_t *instance) { for (unsigned i = 0; i < cmsDeviceCount; i++) { if (cmsDisplayPortSelectNext() == instance) { diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index b05de43173..19c79a1d1e 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -48,7 +48,7 @@ extern displayPort_t *pCurrentDisplay; void cmsInit(void); void cmsHandler(timeUs_t currentTimeUs); -bool cmsDisplayPortSelect(displayPort_t *instance); +bool cmsDisplayPortSelect(const displayPort_t *instance); void cmsMenuOpen(void); const void *cmsMenuChange(displayPort_t *pPort, const void *ptr); const void *cmsMenuExit(displayPort_t *pPort, const void *ptr); diff --git a/src/main/common/maths.c b/src/main/common/maths.c index a287adcefb..c0327454fd 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -229,7 +229,7 @@ void applyMatrixRotation(float *v, fp_rotationMatrix_t *rotationMatrix) #define QMF_SORTF(a,b) { if ((a)>(b)) QMF_SWAPF((a),(b)); } #define QMF_SWAPF(a,b) { float temp=(a);(a)=(b);(b)=temp; } -int32_t quickMedianFilter3(int32_t * v) +int32_t quickMedianFilter3(const int32_t * v) { int32_t p[3]; QMF_COPY(p, v, 3); @@ -238,7 +238,7 @@ int32_t quickMedianFilter3(int32_t * v) return p[1]; } -int32_t quickMedianFilter5(int32_t * v) +int32_t quickMedianFilter5(const int32_t * v) { int32_t p[5]; QMF_COPY(p, v, 5); @@ -249,7 +249,7 @@ int32_t quickMedianFilter5(int32_t * v) return p[2]; } -int32_t quickMedianFilter7(int32_t * v) +int32_t quickMedianFilter7(const int32_t * v) { int32_t p[7]; QMF_COPY(p, v, 7); @@ -262,7 +262,7 @@ int32_t quickMedianFilter7(int32_t * v) return p[3]; } -int32_t quickMedianFilter9(int32_t * v) +int32_t quickMedianFilter9(const int32_t * v) { int32_t p[9]; QMF_COPY(p, v, 9); @@ -277,7 +277,7 @@ int32_t quickMedianFilter9(int32_t * v) return p[4]; } -float quickMedianFilter3f(float * v) +float quickMedianFilter3f(const float * v) { float p[3]; QMF_COPY(p, v, 3); @@ -286,7 +286,7 @@ float quickMedianFilter3f(float * v) return p[1]; } -float quickMedianFilter5f(float * v) +float quickMedianFilter5f(const float * v) { float p[5]; QMF_COPY(p, v, 5); @@ -297,7 +297,7 @@ float quickMedianFilter5f(float * v) return p[2]; } -float quickMedianFilter7f(float * v) +float quickMedianFilter7f(const float * v) { float p[7]; QMF_COPY(p, v, 7); @@ -310,7 +310,7 @@ float quickMedianFilter7f(float * v) return p[3]; } -float quickMedianFilter9f(float * v) +float quickMedianFilter9f(const float * v) { float p[9]; QMF_COPY(p, v, 9); @@ -325,7 +325,7 @@ float quickMedianFilter9f(float * v) return p[4]; } -void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count) +void arraySubInt32(int32_t *dest, const int32_t *array1, const int32_t *array2, int count) { for (int i = 0; i < count; i++) { dest[i] = array1[i] - array2[i]; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 881381b274..26941a685f 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -120,15 +120,15 @@ float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float des void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation); void applyMatrixRotation(float *v, fp_rotationMatrix_t *rotationMatrix); -int32_t quickMedianFilter3(int32_t * v); -int32_t quickMedianFilter5(int32_t * v); -int32_t quickMedianFilter7(int32_t * v); -int32_t quickMedianFilter9(int32_t * v); +int32_t quickMedianFilter3(const int32_t * v); +int32_t quickMedianFilter5(const int32_t * v); +int32_t quickMedianFilter7(const int32_t * v); +int32_t quickMedianFilter9(const int32_t * v); -float quickMedianFilter3f(float * v); -float quickMedianFilter5f(float * v); -float quickMedianFilter7f(float * v); -float quickMedianFilter9f(float * v); +float quickMedianFilter3f(const float * v); +float quickMedianFilter5f(const float * v); +float quickMedianFilter7f(const float * v); +float quickMedianFilter9f(const float * v); #if defined(FAST_MATH) || defined(VERY_FAST_MATH) float sin_approx(float x); @@ -150,7 +150,7 @@ float pow_approx(float a, float b); #define pow_approx(a, b) powf(b, a) #endif -void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count); +void arraySubInt32(int32_t *dest, const int32_t *array1, const int32_t *array2, int count); int16_t qPercent(fix12_t q); int16_t qMultiply(fix12_t q, int16_t input); diff --git a/src/main/common/sdft.c b/src/main/common/sdft.c index f0b496265b..c4233b7435 100644 --- a/src/main/common/sdft.c +++ b/src/main/common/sdft.c @@ -41,9 +41,8 @@ void sdftInit(sdft_t *sdft, const int startBin, const int endBin, const int numB if (!isInitialized) { rPowerN = powf(SDFT_R, SDFT_SAMPLE_SIZE); const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE; - float phi = 0.0f; for (int i = 0; i < SDFT_BIN_COUNT; i++) { - phi = c * i; + float phi = c * i; twiddle[i] = SDFT_R * (cos_approx(phi) + _Complex_I * sin_approx(phi)); } isInitialized = true; diff --git a/src/main/common/time.c b/src/main/common/time.c index 1bde8c1e9d..658558c6b7 100644 --- a/src/main/common/time.c +++ b/src/main/common/time.c @@ -184,12 +184,12 @@ rtcTime_t rtcTimeMake(int32_t secs, uint16_t millis) return ((rtcTime_t)secs) * MILLIS_PER_SECOND + millis; } -int32_t rtcTimeGetSeconds(rtcTime_t *t) +int32_t rtcTimeGetSeconds(const rtcTime_t *t) { return *t / MILLIS_PER_SECOND; } -uint16_t rtcTimeGetMillis(rtcTime_t *t) +uint16_t rtcTimeGetMillis(const rtcTime_t *t) { return *t % MILLIS_PER_SECOND; } @@ -244,7 +244,7 @@ bool rtcGet(rtcTime_t *t) return true; } -bool rtcSet(rtcTime_t *t) +bool rtcSet(const rtcTime_t *t) { started = *t - millis(); return true; diff --git a/src/main/common/time.h b/src/main/common/time.h index cd6a9e40ae..33121739ab 100644 --- a/src/main/common/time.h +++ b/src/main/common/time.h @@ -62,8 +62,8 @@ PG_DECLARE(timeConfig_t, timeConfig); typedef int64_t rtcTime_t; rtcTime_t rtcTimeMake(int32_t secs, uint16_t millis); -int32_t rtcTimeGetSeconds(rtcTime_t *t); -uint16_t rtcTimeGetMillis(rtcTime_t *t); +int32_t rtcTimeGetSeconds(const rtcTime_t *t); +uint16_t rtcTimeGetMillis(const rtcTime_t *t); typedef struct _dateTime_s { // full year @@ -96,7 +96,7 @@ bool dateTimeSplitFormatted(char *formatted, char **date, char **time); bool rtcHasTime(void); bool rtcGet(rtcTime_t *t); -bool rtcSet(rtcTime_t *t); +bool rtcSet(const rtcTime_t *t); bool rtcGetDateTime(dateTime_t *dt); bool rtcSetDateTime(dateTime_t *dt); diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index ff7b0d8fd3..0c87bba16d 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -388,8 +388,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyro IOConfigGPIO(gyro->dev.busType_u.spi.csnPin, SPI_IO_CS_CFG); IOHi(gyro->dev.busType_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus. - uint8_t sensor = MPU_NONE; - // Allow 100ms before attempting to access gyro's SPI bus // Do this once here rather than in each detection routine to speed boot while (millis() < GYRO_SPI_STARTUP_MS); @@ -402,7 +400,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyro // May need a bitmap of hardware to detection function to do it right? for (size_t index = 0 ; gyroSpiDetectFnTable[index] ; index++) { - sensor = (gyroSpiDetectFnTable[index])(&gyro->dev); + uint8_t sensor = (gyroSpiDetectFnTable[index])(&gyro->dev); if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; busDeviceRegister(&gyro->dev); diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 14872fcaf8..68ee4f4d4e 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -55,7 +55,7 @@ uint8_t adcChannelByTag(ioTag_t ioTag) return 0; } -ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) +ADCDevice adcDeviceByInstance(const ADC_TypeDef *instance) { if (instance == ADC1) { return ADCDEV_1; diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 3487c4a65a..15fdadc165 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -114,5 +114,5 @@ int16_t adcInternalComputeTemperature(uint16_t tempAdcValue, uint16_t vrefValue) #endif #if !defined(SIMULATOR_BUILD) -ADCDevice adcDeviceByInstance(ADC_TypeDef *instance); +ADCDevice adcDeviceByInstance(const ADC_TypeDef *instance); #endif diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 6c315282bb..cf0edcc0b2 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -102,7 +102,7 @@ extern adcOperatingConfig_t adcOperatingConfig[ADC_CHANNEL_COUNT]; extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; uint8_t adcChannelByTag(ioTag_t ioTag); -ADCDevice adcDeviceByInstance(ADC_TypeDef *instance); +ADCDevice adcDeviceByInstance(const ADC_TypeDef *instance); bool adcVerifyPin(ioTag_t tag, ADCDevice device); // Marshall values in DMA instance/channel based order to adcChannel based order. diff --git a/src/main/drivers/at32/timer_at32bsp.c b/src/main/drivers/at32/timer_at32bsp.c index 0f059a9b23..8aaf1b22c5 100644 --- a/src/main/drivers/at32/timer_at32bsp.c +++ b/src/main/drivers/at32/timer_at32bsp.c @@ -300,7 +300,7 @@ uint8_t timerLookupChannelIndex(const uint16_t channel) return lookupChannelIndex(channel); } -rccPeriphTag_t timerRCC(tmr_type *tim) +rccPeriphTag_t timerRCC(const tmr_type *tim) { for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { @@ -310,7 +310,7 @@ rccPeriphTag_t timerRCC(tmr_type *tim) return 0; } -uint8_t timerInputIrq(tmr_type *tim) +uint8_t timerInputIrq(const tmr_type *tim) { for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { diff --git a/src/main/drivers/at32/timer_at32f43x.c b/src/main/drivers/at32/timer_at32f43x.c index 2dfedfa9c8..2197317aa8 100644 --- a/src/main/drivers/at32/timer_at32f43x.c +++ b/src/main/drivers/at32/timer_at32f43x.c @@ -190,7 +190,7 @@ const timerHardware_t fullTimerHardware[FULL_TIMER_CHANNEL_COUNT] = { }; #endif -uint32_t timerClock(tmr_type *tim) +uint32_t timerClock(const tmr_type *tim) { UNUSED(tim); return system_core_clock; diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 3cb33e22ba..53cf8d2020 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -50,7 +50,7 @@ static uint8_t spiRegisteredDeviceCount = 0; spiDevice_t spiDevice[SPIDEV_COUNT]; busDevice_t spiBusDevice[SPIDEV_COUNT]; -SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) +SPIDevice spiDeviceByInstance(const SPI_TypeDef *instance) { #ifdef USE_SPI_DEVICE_1 if (instance == SPI1) { diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 8ebc0a9822..d8ba416f8a 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -97,7 +97,7 @@ typedef enum SPIDevice { void spiPreinit(void); void spiPreinitRegister(ioTag_t iotag, uint8_t iocfg, uint8_t init); -void spiPreinitByIO(IO_t io); +void spiPreinitByIO(const IO_t io); void spiPreinitByTag(ioTag_t tag); bool spiInit(SPIDevice device); @@ -106,7 +106,7 @@ bool spiInit(SPIDevice device); void spiInitBusDMA(); -SPIDevice spiDeviceByInstance(SPI_TypeDef *instance); +SPIDevice spiDeviceByInstance(const SPI_TypeDef *instance); SPI_TypeDef *spiInstanceByDevice(SPIDevice device); // BusDevice API diff --git a/src/main/drivers/bus_spi_config.c b/src/main/drivers/bus_spi_config.c index 72c9c16e3b..fc144b2982 100644 --- a/src/main/drivers/bus_spi_config.c +++ b/src/main/drivers/bus_spi_config.c @@ -104,7 +104,7 @@ void spiPreinit(void) } } -void spiPreinitByIO(IO_t io) +void spiPreinitByIO(const IO_t io) { for (int i = 0; i < spiPreinitCount; i++) { if (io == IOGetByTag(spiPreinitArray[i].iotag)) { diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index bbe60b559a..b5984d624d 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -295,7 +295,7 @@ static bool ak8963DirectReadData(const extDevice_t *dev, uint8_t *buf) return ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_HXL, buf, 7); } -static int16_t parseMag(uint8_t *raw, int16_t gain) +static int16_t parseMag(const uint8_t *raw, int16_t gain) { int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256; return constrain(ret, INT16_MIN, INT16_MAX); diff --git a/src/main/drivers/compass/compass_ak8975.c b/src/main/drivers/compass/compass_ak8975.c index 35cb849ccf..aed0d5466c 100644 --- a/src/main/drivers/compass/compass_ak8975.c +++ b/src/main/drivers/compass/compass_ak8975.c @@ -112,7 +112,7 @@ static bool ak8975Init(magDev_t *mag) return true; } -static int16_t parseMag(uint8_t *raw, int16_t gain) +static int16_t parseMag(const uint8_t *raw, int16_t gain) { int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256; return constrain(ret, INT16_MIN, INT16_MAX); diff --git a/src/main/drivers/flash_impl.h b/src/main/drivers/flash_impl.h index 6bd88bd8db..3e2b739836 100644 --- a/src/main/drivers/flash_impl.h +++ b/src/main/drivers/flash_impl.h @@ -109,7 +109,7 @@ typedef struct flashVTable_s { void (*eraseCompletely)(flashDevice_t *fdevice); void (*pageProgramBegin)(flashDevice_t *fdevice, uint32_t address, void (*callback)(uint32_t length)); - uint32_t (*pageProgramContinue)(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount); + uint32_t (*pageProgramContinue)(flashDevice_t *fdevice, uint8_t const **buffers, const uint32_t *bufferSizes, uint32_t bufferCount); void (*pageProgramFinish)(flashDevice_t *fdevice); void (*pageProgram)(flashDevice_t *fdevice, uint32_t address, const uint8_t *data, uint32_t length, void (*callback)(uint32_t length)); diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 25fde81d38..5f1b29233e 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -409,7 +409,7 @@ static void m25p16_pageProgramBegin(flashDevice_t *fdevice, uint32_t address, vo } -static uint32_t m25p16_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount) +static uint32_t m25p16_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, const uint32_t *bufferSizes, uint32_t bufferCount) { // The segment list cannot be in automatic storage as this routine is non-blocking STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 }; @@ -505,7 +505,7 @@ static void m25p16_pageProgram(flashDevice_t *fdevice, uint32_t address, const u #ifdef USE_QUADSPI // Page programming QSPI mode -static uint32_t m25p16_pageProgramContinueQspi(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount) +static uint32_t m25p16_pageProgramContinueQspi(flashDevice_t *fdevice, uint8_t const **buffers, const uint32_t *bufferSizes, uint32_t bufferCount) { if (bufferCount == 0) { return 0; diff --git a/src/main/drivers/flash_w25m.c b/src/main/drivers/flash_w25m.c index 34d19ea21a..16882e4a76 100644 --- a/src/main/drivers/flash_w25m.c +++ b/src/main/drivers/flash_w25m.c @@ -206,7 +206,7 @@ void w25m_pageProgramBegin(flashDevice_t *fdevice, uint32_t address, void (*call dieDevice[currentWriteDie].vTable->pageProgramBegin(&dieDevice[currentWriteDie], address, callback); } -uint32_t w25m_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount) +uint32_t w25m_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, const uint32_t *bufferSizes, uint32_t bufferCount) { UNUSED(fdevice); diff --git a/src/main/drivers/flash_w25n01g.c b/src/main/drivers/flash_w25n01g.c index 84c6bc2d66..c1283bccbc 100644 --- a/src/main/drivers/flash_w25n01g.c +++ b/src/main/drivers/flash_w25n01g.c @@ -547,7 +547,7 @@ void w25n01g_pageProgramBegin(flashDevice_t *fdevice, uint32_t address, void (*c } } -uint32_t w25n01g_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount) +uint32_t w25n01g_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, const uint32_t *bufferSizes, uint32_t bufferCount) { if (bufferCount < 1) { fdevice->callback(0); @@ -650,7 +650,7 @@ busStatus_e w25n01g_callbackWriteComplete(uint32_t arg) return BUS_READY; } -uint32_t w25n01g_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount) +uint32_t w25n01g_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, const uint32_t *bufferSizes, uint32_t bufferCount) { if (bufferCount < 1) { fdevice->callback(0); diff --git a/src/main/drivers/flash_w25q128fv.c b/src/main/drivers/flash_w25q128fv.c index e8fb03bf13..4e09815020 100644 --- a/src/main/drivers/flash_w25q128fv.c +++ b/src/main/drivers/flash_w25q128fv.c @@ -406,7 +406,7 @@ MMFLASH_CODE static void w25q128fv_pageProgramBegin(flashDevice_t *fdevice, uint w25q128fvState.currentWriteAddress = address; } -MMFLASH_CODE static uint32_t w25q128fv_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount) +MMFLASH_CODE static uint32_t w25q128fv_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, const uint32_t *bufferSizes, uint32_t bufferCount) { for (uint32_t i = 0; i < bufferCount; i++) { w25q128fv_waitForReady(fdevice); diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/main/drivers/pwm_output_dshot_shared.c index a8ba64ae85..aed9c2a7c0 100644 --- a/src/main/drivers/pwm_output_dshot_shared.c +++ b/src/main/drivers/pwm_output_dshot_shared.c @@ -146,7 +146,7 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value) void dshotEnableChannels(uint8_t motorCount); -static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count) +static uint32_t decodeTelemetryPacket(const uint32_t buffer[], uint32_t count) { uint32_t value = 0; uint32_t oldValue = buffer[0]; diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index f6c6551fed..6ffa2e386b 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -58,7 +58,7 @@ static void onClose(dyad_Event *e) tcpPort_t* s = (tcpPort_t*)(e->udata); s->clientCount--; s->conn = NULL; - fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount); + fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1U, s->connected, s->clientCount); if (s->clientCount == 0) { s->connected = false; } @@ -66,7 +66,7 @@ static void onClose(dyad_Event *e) static void onAccept(dyad_Event *e) { tcpPort_t* s = (tcpPort_t*)(e->udata); - fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1, s->clientCount); + fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1U, s->clientCount); s->connected = true; if (s->clientCount > 0) { @@ -74,7 +74,7 @@ static void onAccept(dyad_Event *e) return; } s->clientCount++; - fprintf(stderr, "[NEW]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount); + fprintf(stderr, "[NEW]UART%u: %d,%d\n", s->id + 1U, s->connected, s->clientCount); s->conn = e->remote; dyad_setNoDelay(e->remote, 1); dyad_setTimeout(e->remote, 120); diff --git a/src/main/drivers/stm32/bus_spi_ll.c b/src/main/drivers/stm32/bus_spi_ll.c index aafdc4d674..463b3a3640 100644 --- a/src/main/drivers/stm32/bus_spi_ll.c +++ b/src/main/drivers/stm32/bus_spi_ll.c @@ -74,7 +74,7 @@ static LL_SPI_InitTypeDef defaultInit = .ClockPhase = LL_SPI_PHASE_2EDGE, }; -static uint32_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor) +static uint32_t spiDivisorToBRbits(const SPI_TypeDef *instance, uint16_t divisor) { #if !defined(STM32H7) // SPI2 and SPI3 are on APB1/AHB1 which PCLK is half that of APB2/AHB2. diff --git a/src/main/drivers/stm32/bus_spi_stdperiph.c b/src/main/drivers/stm32/bus_spi_stdperiph.c index e491a4cb04..988fc79876 100644 --- a/src/main/drivers/stm32/bus_spi_stdperiph.c +++ b/src/main/drivers/stm32/bus_spi_stdperiph.c @@ -52,7 +52,7 @@ static SPI_InitTypeDef defaultInit = { .SPI_CPHA = SPI_CPHA_2Edge }; -static uint16_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor) +static uint16_t spiDivisorToBRbits(const SPI_TypeDef *instance, uint16_t divisor) { // SPI2 and SPI3 are on APB1/AHB1 which PCLK is half that of APB2/AHB2. #if defined(STM32F410xx) || defined(STM32F411xE) diff --git a/src/main/drivers/stm32/serial_uart_hal.c b/src/main/drivers/stm32/serial_uart_hal.c index 6913df56ec..5fed1aa031 100644 --- a/src/main/drivers/stm32/serial_uart_hal.c +++ b/src/main/drivers/stm32/serial_uart_hal.c @@ -67,7 +67,7 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) } } -static uartDevice_t *uartFindDevice(uartPort_t *uartPort) +static uartDevice_t *uartFindDevice(const uartPort_t *uartPort) { for (uint32_t i = 0; i < UARTDEV_COUNT_MAX; i++) { uartDevice_t *candidate = uartDevmap[i]; diff --git a/src/main/drivers/stm32/timer_hal.c b/src/main/drivers/stm32/timer_hal.c index f3862a6f3f..242ac89141 100644 --- a/src/main/drivers/stm32/timer_hal.c +++ b/src/main/drivers/stm32/timer_hal.c @@ -309,7 +309,7 @@ uint8_t timerLookupChannelIndex(const uint16_t channel) return lookupChannelIndex(channel); } -rccPeriphTag_t timerRCC(TIM_TypeDef *tim) +rccPeriphTag_t timerRCC(const TIM_TypeDef *tim) { for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { @@ -319,7 +319,7 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim) return 0; } -uint8_t timerInputIrq(TIM_TypeDef *tim) +uint8_t timerInputIrq(const TIM_TypeDef *tim) { for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { diff --git a/src/main/drivers/stm32/timer_stdperiph.c b/src/main/drivers/stm32/timer_stdperiph.c index 4b809c81aa..b225072caa 100644 --- a/src/main/drivers/stm32/timer_stdperiph.c +++ b/src/main/drivers/stm32/timer_stdperiph.c @@ -290,7 +290,7 @@ uint8_t timerLookupChannelIndex(const uint16_t channel) return lookupChannelIndex(channel); } -rccPeriphTag_t timerRCC(TIM_TypeDef *tim) +rccPeriphTag_t timerRCC(const TIM_TypeDef *tim) { for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { @@ -300,7 +300,7 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim) return 0; } -uint8_t timerInputIrq(TIM_TypeDef *tim) +uint8_t timerInputIrq(const TIM_TypeDef *tim) { for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { diff --git a/src/main/drivers/stm32/timer_stm32f4xx.c b/src/main/drivers/stm32/timer_stm32f4xx.c index 53fa469ce7..bd1218c4d6 100644 --- a/src/main/drivers/stm32/timer_stm32f4xx.c +++ b/src/main/drivers/stm32/timer_stm32f4xx.c @@ -224,7 +224,7 @@ const timerHardware_t fullTimerHardware[FULL_TIMER_CHANNEL_COUNT] = { 7 TIM8_CH1 TIM8_CH2 TIM8_CH3 TIM8_CH4 */ -uint32_t timerClock(TIM_TypeDef *tim) +uint32_t timerClock(const TIM_TypeDef *tim) { #if defined(STM32F411xE) UNUSED(tim); diff --git a/src/main/drivers/stm32/timer_stm32f7xx.c b/src/main/drivers/stm32/timer_stm32f7xx.c index 8862aa3b82..fc36bc15cd 100644 --- a/src/main/drivers/stm32/timer_stm32f7xx.c +++ b/src/main/drivers/stm32/timer_stm32f7xx.c @@ -203,7 +203,7 @@ const timerHardware_t fullTimerHardware[FULL_TIMER_CHANNEL_COUNT] = { 7 TIM8_CH1 TIM8_CH2 TIM8_CH3 TIM8_CH4 */ -uint32_t timerClock(TIM_TypeDef *tim) +uint32_t timerClock(const TIM_TypeDef *tim) { UNUSED(tim); return SystemCoreClock; diff --git a/src/main/drivers/stm32/timer_stm32g4xx.c b/src/main/drivers/stm32/timer_stm32g4xx.c index f55d87bdfd..eb3de08300 100644 --- a/src/main/drivers/stm32/timer_stm32g4xx.c +++ b/src/main/drivers/stm32/timer_stm32g4xx.c @@ -169,7 +169,7 @@ const timerHardware_t fullTimerHardware[FULL_TIMER_CHANNEL_COUNT] = { }; #endif -uint32_t timerClock(TIM_TypeDef *tim) +uint32_t timerClock(const TIM_TypeDef *tim) { /* * RM0440 Rev.1 diff --git a/src/main/drivers/stm32/timer_stm32h7xx.c b/src/main/drivers/stm32/timer_stm32h7xx.c index 5e3a5976a3..e4119a109c 100644 --- a/src/main/drivers/stm32/timer_stm32h7xx.c +++ b/src/main/drivers/stm32/timer_stm32h7xx.c @@ -161,7 +161,7 @@ const timerHardware_t fullTimerHardware[FULL_TIMER_CHANNEL_COUNT] = { #endif -uint32_t timerClock(TIM_TypeDef *tim) +uint32_t timerClock(const TIM_TypeDef *tim) { int timpre; uint32_t pclk; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 9854aec4b8..ed4d85212a 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -173,13 +173,13 @@ void timerForceOverflow(TIM_TypeDef *tim); void timerConfigUpdateCallback(const TIM_TypeDef *tim, timerOvrHandlerRec_t *updateCallback); -uint32_t timerClock(TIM_TypeDef *tim); +uint32_t timerClock(const TIM_TypeDef *tim); void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz); // TODO - just for migration void timerReconfigureTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz); -rccPeriphTag_t timerRCC(TIM_TypeDef *tim); -uint8_t timerInputIrq(TIM_TypeDef *tim); +rccPeriphTag_t timerRCC(const TIM_TypeDef *tim); +uint8_t timerInputIrq(const TIM_TypeDef *tim); #if defined(USE_TIMER_MGMT) extern const resourceOwner_t freeOwner; diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index fe21ad8208..ef3b5d2640 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -74,7 +74,7 @@ bool IS_RC_MODE_ACTIVE(boxId_e boxId) return bitArrayGet(&rcModeActivationMask, boxId); } -void rcModeUpdate(boxBitmask_t *newState) +void rcModeUpdate(const boxBitmask_t *newState) { rcModeActivationMask = *newState; } diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 0b8e2d36b5..f50c59b373 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -141,7 +141,7 @@ typedef struct modeActivationProfile_s { #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) bool IS_RC_MODE_ACTIVE(boxId_e boxId); -void rcModeUpdate(boxBitmask_t *newState); +void rcModeUpdate(const boxBitmask_t *newState); bool airmodeIsEnabled(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 8f4e41d72b..aea3f1a88f 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -323,7 +323,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) } } -static bool imuIsAccelerometerHealthy(float *accAverage) +static bool imuIsAccelerometerHealthy(const float *accAverage) { float accMagnitudeSq = 0; for (int axis = 0; axis < 3; axis++) { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6faed644d1..e14589d0ea 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -388,7 +388,7 @@ static void applyRpmLimiter(mixerRuntime_t *mixer) } #endif // USE_RPM_LIMIT -static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t *activeMixer) +static void applyMixToMotors(const float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t *activeMixer) { // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e7f818cb86..0660209cd8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -593,7 +593,7 @@ static float accelerationLimit(int axis, float currentPidSetpoint) return currentPidSetpoint; } -static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT]) +static void rotateVector(float v[XYZ_AXIS_COUNT], const float rotation[XYZ_AXIS_COUNT]) { // rotate v around rotation vector rotation // rotation in radians, all elements must be small diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index 50408bd1b9..97c6f7fe92 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -607,7 +607,7 @@ static uint8_t *afatfs_cacheSectorGetMemory(int cacheEntryIndex) return afatfs.cache + cacheEntryIndex * AFATFS_SECTOR_SIZE; } -static int afatfs_getCacheDescriptorIndexForBuffer(uint8_t *memory) +static int afatfs_getCacheDescriptorIndexForBuffer(const uint8_t *memory) { int index = (memory - afatfs.cache) / AFATFS_SECTOR_SIZE; @@ -2294,7 +2294,7 @@ static afatfsOperationStatus_e afatfs_extendSubdirectoryContinue(afatfsFile_t *d } // Seek back to the beginning of the cluster - afatfs_assert(afatfs_fseekAtomic(directory, -AFATFS_SECTOR_SIZE * ((int32_t)afatfs.sectorsPerCluster - 1))); + afatfs_assert(afatfs_fseekAtomic(directory, -(AFATFS_SECTOR_SIZE * ((int32_t)afatfs.sectorsPerCluster - 1)))); opState->phase = AFATFS_EXTEND_SUBDIRECTORY_PHASE_SUCCESS; goto doMore; break; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 9920af9fa9..d89fecc687 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -556,58 +556,63 @@ static void ubloxSendByteUpdateChecksum(const uint8_t data, uint8_t *checksumA, serialWrite(gpsPort, data); } -static void ubloxSendDataUpdateChecksum(const uint8_t *data, uint8_t len, uint8_t *checksumA, uint8_t *checksumB) +static void ubloxSendDataUpdateChecksum(const ubxMessage_t *msg, uint8_t *checksumA, uint8_t *checksumB) { + // CRC includes msg_class, msg_id, length and payload + // length is payload length only + const uint8_t *data = (const uint8_t *)&msg->header.msg_class; + uint16_t len = msg->header.length + sizeof(msg->header.msg_class) + sizeof(msg->header.msg_id) + sizeof(msg->header.length); + while (len--) { ubloxSendByteUpdateChecksum(*data, checksumA, checksumB); data++; } } -static void ubloxSendMessage(const uint8_t *data, uint8_t len, bool skipAck) +static void ubloxSendMessage(const ubxMessage_t *msg, bool skipAck) { uint8_t checksumA = 0, checksumB = 0; - serialWrite(gpsPort, data[0]); - serialWrite(gpsPort, data[1]); - ubloxSendDataUpdateChecksum(&data[2], len - 2, &checksumA, &checksumB); + serialWrite(gpsPort, msg->header.preamble1); + serialWrite(gpsPort, msg->header.preamble2); + ubloxSendDataUpdateChecksum(msg, &checksumA, &checksumB); serialWrite(gpsPort, checksumA); serialWrite(gpsPort, checksumB); // Save state for ACK waiting - gpsData.ackWaitingMsgId = data[3]; //save message id for ACK + gpsData.ackWaitingMsgId = msg->header.msg_id; //save message id for ACK gpsData.ackState = skipAck ? UBLOX_ACK_GOT_ACK : UBLOX_ACK_WAITING; gpsData.lastMessageSent = gpsData.now; } static void ubloxSendClassMessage(ubxProtocolBytes_e class_id, ubxProtocolBytes_e msg_id, uint16_t length) { - ubxMessage_t tx_buffer; - tx_buffer.header.preamble1 = PREAMBLE1; - tx_buffer.header.preamble2 = PREAMBLE2; - tx_buffer.header.msg_class = class_id; - tx_buffer.header.msg_id = msg_id; - tx_buffer.header.length = length; - ubloxSendMessage((const uint8_t *) &tx_buffer, length + 6, false); + ubxMessage_t msg; + msg.header.preamble1 = PREAMBLE1; + msg.header.preamble2 = PREAMBLE2; + msg.header.msg_class = class_id; + msg.header.msg_id = msg_id; + msg.header.length = length; + ubloxSendMessage(&msg, false); } -static void ubloxSendConfigMessage(ubxMessage_t *message, uint8_t msg_id, uint8_t length, bool skipAck) +static void ubloxSendConfigMessage(ubxMessage_t *msg, uint8_t msg_id, uint8_t length, bool skipAck) { - message->header.preamble1 = PREAMBLE1; - message->header.preamble2 = PREAMBLE2; - message->header.msg_class = CLASS_CFG; - message->header.msg_id = msg_id; - message->header.length = length; - ubloxSendMessage((const uint8_t *) message, length + 6, skipAck); + msg->header.preamble1 = PREAMBLE1; + msg->header.preamble2 = PREAMBLE2; + msg->header.msg_class = CLASS_CFG; + msg->header.msg_id = msg_id; + msg->header.length = length; + ubloxSendMessage(msg, skipAck); } static void ubloxSendPollMessage(uint8_t msg_id) { - ubxMessage_t tx_buffer; - tx_buffer.header.preamble1 = PREAMBLE1; - tx_buffer.header.preamble2 = PREAMBLE2; - tx_buffer.header.msg_class = CLASS_CFG; - tx_buffer.header.msg_id = msg_id; - tx_buffer.header.length = 0; - ubloxSendMessage((const uint8_t *) &tx_buffer, 6, false); + ubxMessage_t msg; + msg.header.preamble1 = PREAMBLE1; + msg.header.preamble2 = PREAMBLE2; + msg.header.msg_class = CLASS_CFG; + msg.header.msg_id = msg_id; + msg.header.length = 0; + ubloxSendMessage(&msg, false); } static void ubloxSendNAV5Message(uint8_t model) { @@ -2568,7 +2573,7 @@ void GPS_reset_home_position(void) #define TAN_89_99_DEGREES 5729.57795f // Get distance between two points in cm // Get bearing from pos1 to pos2, returns an 1deg = 100 precision -void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing) +void GPS_distance_cm_bearing(const int32_t *currentLat1, const int32_t *currentLon1, const int32_t *destinationLat2, const int32_t *destinationLon2, uint32_t *dist, int32_t *bearing) { float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown; diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 1054b55cfc..421929041b 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -393,7 +393,7 @@ void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort); void onGpsNewData(void); void GPS_reset_home_position(void); void GPS_calc_longitude_scaling(int32_t lat); -void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing); +void GPS_distance_cm_bearing(const int32_t *currentLat1, const int32_t *currentLon1, const int32_t *destinationLat2, const int32_t *destinationLon2, uint32_t *dist, int32_t *bearing); void gpsSetFixState(bool state); float getGpsDataIntervalSeconds(void); // sends GPS Nav Data interval to GPS Rescue baudRate_e getGpsPortActualBaudRateIndex(void); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 25a027b29e..0a13d9f7df 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -796,7 +796,6 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer) } uint8_t band = 255, channel = 255; - uint16_t check = 0; if (updateNow) { // keep counter running, so it stays in sync with vtx @@ -807,7 +806,7 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer) frequency = vtxCommonLookupFrequency(vtxDevice, band, channel); // check if last vtx values have changed. - check = ((vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0) + (power << 1) + (band << 4) + (channel << 8); + uint16_t check = ((vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0) + (power << 1) + (band << 4) + (channel << 8); if (!showSettings && check != lastCheck) { // display settings for 3 seconds. showSettings = 15; @@ -821,23 +820,21 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer) *timer += HZ_TO_US(LED_OVERLAY_VTX_RATE_HZ); } - hsvColor_t color = {0, 0, 0}; if (showSettings) { // show settings uint8_t vtxLedCount = 0; for (int i = 0; i < ledCounts.count && vtxLedCount < 6; ++i) { const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[i]; if (ledGetOverlayBit(ledConfig, LED_OVERLAY_VTX)) { + hsvColor_t color = {0, 0, 0}; if (vtxLedCount == 0) { color.h = HSV(GREEN).h; color.s = HSV(GREEN).s; color.v = blink ? 15 : 0; // blink received settings - } - else if (vtxLedCount > 0 && power >= vtxLedCount && !(vtxStatus & VTX_STATUS_PIT_MODE)) { // show power + } else if (vtxLedCount > 0 && power >= vtxLedCount && !(vtxStatus & VTX_STATUS_PIT_MODE)) { // show power color.h = HSV(ORANGE).h; color.s = HSV(ORANGE).s; color.v = blink ? 15 : 0; // blink received settings - } - else { // turn rest off + } else { // turn rest off color.h = HSV(BLACK).h; color.s = HSV(BLACK).s; color.v = HSV(BLACK).v; diff --git a/src/main/io/rcdevice.c b/src/main/io/rcdevice.c index f747874961..793d5c7972 100644 --- a/src/main/io/rcdevice.c +++ b/src/main/io/rcdevice.c @@ -86,7 +86,7 @@ static uint8_t runcamDeviceGetRespLen(uint8_t command) return 0; } -static bool rcdeviceRespCtxQueuePush(rcdeviceWaitingResponseQueue *queue, rcdeviceResponseParseContext_t *respCtx) +static bool rcdeviceRespCtxQueuePush(rcdeviceWaitingResponseQueue *queue, const rcdeviceResponseParseContext_t *respCtx) { if (queue == NULL || (queue->itemCount + 1) > MAX_WAITING_RESPONSES) { return false; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index ddfbd87418..3266da2165 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -253,7 +253,7 @@ serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identi return NULL; } -serialPortUsage_t *findSerialPortUsageByPort(serialPort_t *serialPort) +serialPortUsage_t *findSerialPortUsageByPort(const serialPort_t *serialPort) { uint8_t index; for (index = 0; index < SERIAL_PORT_COUNT; index++) { diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 425c1f619c..03aff469a4 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -110,7 +110,7 @@ static uint8_t suart_getc_(uint8_t *bt) return 1; } -static void suart_putc_(uint8_t *tx_b) +static void suart_putc_(const uint8_t *tx_b) { // shift out stopbit first uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10); @@ -132,7 +132,7 @@ static void suart_putc_(uint8_t *tx_b) static uint8_16_u CRC_16; static uint8_16_u LastCRC_16; -static void ByteCrc(uint8_t *bt) +static void ByteCrc(const uint8_t *bt) { uint8_t xb = *bt; for (uint8_t i = 0; i < 8; i++) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 9006909ba3..8d2ce1af32 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -549,7 +549,7 @@ static void saResendCmd(void) saSendFrame(sa_osbuf, sa_oslen); } -static void saSendCmd(uint8_t *buf, int len) +static void saSendCmd(const uint8_t *buf, int len) { for (int i = 0 ; i < len ; i++) { sa_osbuf[i] = buf[i]; diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index f2abcbdc2d..3fa87c7e9b 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -140,7 +140,7 @@ static int trampReceivePos = 0; static timeUs_t trampLastTimeUs = 0; // Calculate tramp protocol checksum of provided buffer -static uint8_t trampChecksum(uint8_t *trampBuf) +static uint8_t trampChecksum(const uint8_t *trampBuf) { uint8_t cksum = 0; diff --git a/src/main/rx/cc2500_frsky_d.c b/src/main/rx/cc2500_frsky_d.c index 3ac9eae37f..ec34cd1307 100644 --- a/src/main/rx/cc2500_frsky_d.c +++ b/src/main/rx/cc2500_frsky_d.c @@ -114,7 +114,7 @@ static void frSkyDTelemetryWriteByte(const char data) } #endif -static void buildTelemetryFrame(uint8_t *packet) +static void buildTelemetryFrame(const uint8_t *packet) { uint8_t a1Value; switch (rxCc2500SpiConfig()->a1Source) { diff --git a/src/main/rx/cc2500_frsky_shared.c b/src/main/rx/cc2500_frsky_shared.c index 8c08625a7e..db171a4043 100644 --- a/src/main/rx/cc2500_frsky_shared.c +++ b/src/main/rx/cc2500_frsky_shared.c @@ -235,7 +235,7 @@ static void initTuneRx(void) cc2500Strobe(CC2500_SRX); } -static bool isValidBindPacket(uint8_t *packet) +static bool isValidBindPacket(const uint8_t *packet) { if (spiProtocol == RX_SPI_FRSKY_D || spiProtocol == RX_SPI_FRSKY_X_V2 || spiProtocol == RX_SPI_FRSKY_X_LBT_V2) { if (!(packet[packetLength - 1] & 0x80)) { diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index d0682cece4..77bfbcd7a5 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -93,13 +93,12 @@ uint8_t jetiExBusRequestFrame[EXBUS_MAX_REQUEST_FRAME_SIZE]; static uint16_t jetiExBusChannelData[JETIEXBUS_CHANNEL_COUNT]; // Jeti Ex Bus CRC calculations for a frame -uint16_t jetiExBusCalcCRC16(uint8_t *pt, uint8_t msgLen) +uint16_t jetiExBusCalcCRC16(const uint8_t *pt, uint8_t msgLen) { uint16_t crc16_data = 0; - uint8_t data=0; for (uint8_t mlen = 0; mlen < msgLen; mlen++) { - data = pt[mlen] ^ ((uint8_t)(crc16_data) & (uint8_t)(0xFF)); + uint8_t data = pt[mlen] ^ (crc16_data & 0xff); data ^= data << 4; crc16_data = ((((uint16_t)data << 8) | ((crc16_data & 0xFF00) >> 8)) ^ (uint8_t)(data >> 4) diff --git a/src/main/rx/jetiexbus.h b/src/main/rx/jetiexbus.h index ba139b0f59..4e8e6350f6 100644 --- a/src/main/rx/jetiexbus.h +++ b/src/main/rx/jetiexbus.h @@ -51,5 +51,5 @@ extern uint8_t jetiExBusRequestFrame[EXBUS_MAX_REQUEST_FRAME_SIZE]; struct serialPort_s; extern struct serialPort_s *jetiExBusPort; -uint16_t jetiExBusCalcCRC16(uint8_t *pt, uint8_t msgLen); +uint16_t jetiExBusCalcCRC16(const uint8_t *pt, uint8_t msgLen); bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState); diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index 19dd233060..d21d07c9ed 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -46,7 +46,7 @@ float rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan) /* * Called from MSP command handler - mspFcProcessCommand */ -void rxMspFrameReceive(uint16_t *frame, int channelCount) +void rxMspFrameReceive(const uint16_t *frame, int channelCount) { for (int i = 0; i < channelCount; i++) { mspFrame[i] = frame[i]; diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index ab765a7770..cb80ac4c30 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -24,5 +24,5 @@ struct rxConfig_s; struct rxRuntimeState_s; float rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan); void rxMspInit(const struct rxConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState); -void rxMspFrameReceive(uint16_t *frame, int channelCount); +void rxMspFrameReceive(const uint16_t *frame, int channelCount); uint8_t rxMspOverrideFrameStatus(); diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 3a985ba201..7890e52537 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -133,7 +133,7 @@ void queueClear(void) taskQueueSize = 0; } -bool queueContains(task_t *task) +bool queueContains(const task_t *task) { for (int ii = 0; ii < taskQueueSize; ++ii) { if (taskQueueArray[ii] == task) { diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 90fedba3b0..c39abce780 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -469,7 +469,7 @@ void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply) *lengthPtr = sbufPtr(dst) - lengthPtr; } -static void crsfProcessSpeedNegotiationCmd(uint8_t *frameStart) +static void crsfProcessSpeedNegotiationCmd(const uint8_t *frameStart) { uint32_t newBaudrate = frameStart[2] << 24 | frameStart[3] << 16 | frameStart[4] << 8 | frameStart[5]; uint8_t ii = 0; diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 4be074ca1c..512f9783c2 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -500,8 +500,7 @@ void hottTextmodeExit(void) void hottTextmodeWriteChar(uint8_t column, uint8_t row, char c) { if (column < HOTT_TEXTMODE_DISPLAY_COLUMNS && row < HOTT_TEXTMODE_DISPLAY_ROWS) { - if (hottTextModeMessage.txt[row][column] != c) - hottTextModeMessage.txt[row][column] = c; + hottTextModeMessage.txt[row][column] = c; } } diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 84282fc1f3..d7862bbc06 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -283,9 +283,8 @@ static int16_t getACC(uint8_t index) static void setCombinedFrame(uint8_t* bufferPtr, const uint8_t* structure, uint8_t itemCount) { uint8_t offset = 0; - uint8_t size = 0; for (unsigned i = 0; i < itemCount; i++) { - size = getSensorLength(structure[i]); + uint8_t size = getSensorLength(structure[i]); setValue(bufferPtr + offset, structure[i], size); offset += size; } @@ -306,11 +305,9 @@ static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value) } if (!result) return result; - uint16_t gpsFixType = 0; - uint16_t sats = 0; if (sensors(SENSOR_GPS)) { - gpsFixType = !STATE(GPS_FIX) ? 1 : (gpsSol.numSat < GPS_MIN_SAT_COUNT ? 2 : 3); - sats = gpsSol.numSat; + uint16_t gpsFixType = !STATE(GPS_FIX) ? 1 : (gpsSol.numSat < GPS_MIN_SAT_COUNT ? 2 : 3); + uint16_t sats = gpsSol.numSat; if (STATE(GPS_FIX) || sensorType == IBUS_SENSOR_TYPE_GPS_STATUS) { result = true; switch (sensorType) { diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 4cb308f747..283c8ebf6a 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -183,7 +183,7 @@ static uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item); static uint8_t getNextActiveSensor(uint8_t currentSensor); // Jeti Ex Telemetry CRC calculations for a frame -uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen) +uint8_t calcCRC8(const uint8_t *pt, uint8_t msgLen) { uint8_t crc=0; for (uint8_t mlen = 0; mlen < msgLen; mlen++) { @@ -455,7 +455,7 @@ uint8_t createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item) return item; // return the next item } -void createExBusMessage(uint8_t *exBusMessage, uint8_t *exMessage, uint8_t packetID) +void createExBusMessage(uint8_t *exBusMessage, const uint8_t *exMessage, uint8_t packetID) { uint16_t crc16; diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc index 2646eb8d6b..5bb86782f5 100644 --- a/src/test/unit/scheduler_unittest.cc +++ b/src/test/unit/scheduler_unittest.cc @@ -105,7 +105,7 @@ extern "C" { extern task_t* taskQueueArray[]; extern void queueClear(void); - extern bool queueContains(task_t *task); + extern bool queueContains(const task_t *task); extern bool queueAdd(task_t *task); extern bool queueRemove(task_t *task); extern task_t *queueFirst(void);