1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-12 19:10:32 +03:00

Make Cppcheck happier revived (#13566)

Co-authored-by: Štěpán Dalecký <daleckystepan@gmail.com>
This commit is contained in:
Mark Haslinghuis 2024-05-10 05:23:32 +02:00 committed by GitHub
parent d5af7d2254
commit 5a28ce5129
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
63 changed files with 145 additions and 157 deletions

View file

@ -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)
{

View file

@ -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),

View file

@ -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) {

View file

@ -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);

View file

@ -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];

View file

@ -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);

View file

@ -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;

View file

@ -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;

View file

@ -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);

View file

@ -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);

View file

@ -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;

View file

@ -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

View file

@ -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.

View file

@ -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) {

View file

@ -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;

View file

@ -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) {

View file

@ -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

View file

@ -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)) {

View file

@ -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);

View file

@ -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);

View file

@ -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));

View file

@ -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;

View file

@ -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);

View file

@ -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);

View file

@ -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);

View file

@ -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];

View file

@ -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);

View file

@ -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.

View file

@ -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)

View file

@ -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];

View file

@ -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) {

View file

@ -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) {

View file

@ -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);

View file

@ -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;

View file

@ -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

View file

@ -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;

View file

@ -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;

View file

@ -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;
}

View file

@ -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);

View file

@ -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++) {

View file

@ -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.

View file

@ -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

View file

@ -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;

View file

@ -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;

View file

@ -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);

View file

@ -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;

View file

@ -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;

View file

@ -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++) {

View file

@ -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++)

View file

@ -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];

View file

@ -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;

View file

@ -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) {

View file

@ -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)) {

View file

@ -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)

View file

@ -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);

View file

@ -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];

View file

@ -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();

View file

@ -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) {

View file

@ -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;

View file

@ -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;
}
}

View file

@ -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) {

View file

@ -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;

View file

@ -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);