diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index fe3ff4efe1..d6ac70dc94 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -938,7 +938,7 @@ static void writeGPSHomeFrame() gpsHistory.GPS_home[1] = GPS_home[1]; } -static void writeGPSFrame(uint32_t currentTime) +static void writeGPSFrame(timeUs_t currentTimeUs) { blackboxWrite('G'); @@ -950,7 +950,7 @@ static void writeGPSFrame(uint32_t currentTime) */ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) { // Predict the time of the last frame in the main log - blackboxWriteUnsignedVB(currentTime - blackboxHistory[1]->time); + blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time); } blackboxWriteUnsignedVB(GPS_numSat); @@ -969,12 +969,12 @@ static void writeGPSFrame(uint32_t currentTime) /** * Fill the current state of the blackbox using values read from the flight controller */ -static void loadMainState(uint32_t currentTime) +static void loadMainState(timeUs_t currentTimeUs) { blackboxMainState_t *blackboxCurrent = blackboxHistory[0]; int i; - blackboxCurrent->time = currentTime; + blackboxCurrent->time = currentTimeUs; for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_P[i] = axisPID_P[i]; @@ -1401,7 +1401,7 @@ static void blackboxAdvanceIterationTimers() } // Called once every FC loop in order to log the current state -static void blackboxLogIteration(uint32_t currentTime) +static void blackboxLogIteration(timeUs_t currentTimeUs) { // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames if (blackboxShouldLogIFrame()) { @@ -1411,7 +1411,7 @@ static void blackboxLogIteration(uint32_t currentTime) */ writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes()); - loadMainState(currentTime); + loadMainState(currentTimeUs); writeIntraframe(); } else { blackboxCheckAndLogArmingBeep(); @@ -1424,7 +1424,7 @@ static void blackboxLogIteration(uint32_t currentTime) */ writeSlowFrameIfNeeded(true); - loadMainState(currentTime); + loadMainState(currentTimeUs); writeInterframe(); } #ifdef GPS @@ -1440,11 +1440,11 @@ static void blackboxLogIteration(uint32_t currentTime) || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) { writeGPSHomeFrame(); - writeGPSFrame(currentTime); + writeGPSFrame(currentTimeUs); } else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0] || GPS_coord[1] != gpsHistory.GPS_coord[1]) { //We could check for velocity changes as well but I doubt it changes independent of position - writeGPSFrame(currentTime); + writeGPSFrame(currentTimeUs); } } #endif @@ -1457,7 +1457,7 @@ static void blackboxLogIteration(uint32_t currentTime) /** * Call each flight loop iteration to perform blackbox logging. */ -void handleBlackbox(uint32_t currentTime) +void handleBlackbox(timeUs_t currentTimeUs) { int i; @@ -1549,12 +1549,12 @@ void handleBlackbox(uint32_t currentTime) flightLogEvent_loggingResume_t resume; resume.logIteration = blackboxIteration; - resume.currentTime = currentTime; + resume.currentTime = currentTimeUs; blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume); blackboxSetState(BLACKBOX_STATE_RUNNING); - blackboxLogIteration(currentTime); + blackboxLogIteration(currentTimeUs); } // Keep the logging timers ticking so our log iteration continues to advance @@ -1566,7 +1566,7 @@ void handleBlackbox(uint32_t currentTime) if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX) && !startedLoggingInTestMode) { blackboxSetState(BLACKBOX_STATE_PAUSED); } else { - blackboxLogIteration(currentTime); + blackboxLogIteration(currentTimeUs); } blackboxAdvanceIterationTimers(); diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index b7fc3c6aa8..1a9690095a 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -19,6 +19,8 @@ #include "blackbox/blackbox_fielddefs.h" +#include "common/time.h" + typedef struct blackboxConfig_s { uint8_t rate_num; uint8_t rate_denom; @@ -29,7 +31,7 @@ typedef struct blackboxConfig_s { void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data); void initBlackbox(void); -void handleBlackbox(uint32_t currentTime); +void handleBlackbox(timeUs_t currentTimeUs); void startBlackbox(void); void finishBlackbox(void); diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 388fd6b037..8ec5c3c3c5 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -897,16 +897,16 @@ static void cmsUpdate(uint32_t currentTimeUs) lastCalledMs = currentTimeMs; } -void cmsHandler(uint32_t currentTime) +void cmsHandler(timeUs_t currentTimeUs) { if (cmsDeviceCount < 0) return; - static uint32_t lastCalled = 0; + static timeUs_t lastCalledUs = 0; - if (currentTime >= lastCalled + CMS_UPDATE_INTERVAL_US) { - lastCalled = currentTime; - cmsUpdate(currentTime); + if (currentTimeUs >= lastCalledUs + CMS_UPDATE_INTERVAL_US) { + lastCalledUs = currentTimeUs; + cmsUpdate(currentTimeUs); } } diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index 8e5110af22..f988ef9b6c 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -2,12 +2,14 @@ #include "drivers/display.h" +#include "common/time.h" + // Device management bool cmsDisplayPortRegister(displayPort_t *pDisplay); // For main.c and scheduler void cmsInit(void); -void cmsHandler(uint32_t currentTime); +void cmsHandler(timeUs_t currentTimeUs); long cmsMenuChange(displayPort_t *pPort, const void *ptr); long cmsMenuExit(displayPort_t *pPort, const void *ptr); diff --git a/src/main/common/time.h b/src/main/common/time.h index 1a1323bc1d..4b32710ba8 100644 --- a/src/main/common/time.h +++ b/src/main/common/time.h @@ -21,6 +21,8 @@ #include "platform.h" +// time difference, 32 bits always sufficient +typedef int32_t timeDelta_t; // millisecond time typedef uint32_t timeMs_t ; // microsecond time @@ -31,3 +33,5 @@ typedef uint64_t timeUs_t; typedef uint32_t timeUs_t; #define TIMEUS_MAX UINT32_MAX #endif + +static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); } diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index c0423f9bec..e432d687c2 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -21,9 +21,9 @@ typedef enum { ADC_BATTERY = 0, - ADC_RSSI = 1, + ADC_CURRENT = 1, ADC_EXTERNAL1 = 2, - ADC_CURRENT = 3, + ADC_RSSI = 3, ADC_CHANNEL_MAX = ADC_CURRENT } AdcChannel; diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index ce33d7b0eb..b411c38457 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -126,6 +126,7 @@ void spiInitDevice(SPIDevice device) IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); if (spi->nss) { + IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device)); IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af); } #endif @@ -135,6 +136,7 @@ void spiInitDevice(SPIDevice device) IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG); if (spi->nss) { + IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device)); IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); } #endif diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 85e58fd0b9..6e2fa44ea9 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -284,17 +284,21 @@ void initActiveBoxIds(void) if (sensors(SENSOR_ACC)) { activeBoxIds[activeBoxIdCount++] = BOXANGLE; activeBoxIds[activeBoxIdCount++] = BOXHORIZON; + activeBoxIds[activeBoxIdCount++] = BOXHEADFREE; } +#ifdef BARO if (sensors(SENSOR_BARO)) { activeBoxIds[activeBoxIdCount++] = BOXBARO; } +#endif - if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { +#ifdef MAG + if (sensors(SENSOR_MAG)) { activeBoxIds[activeBoxIdCount++] = BOXMAG; - activeBoxIds[activeBoxIdCount++] = BOXHEADFREE; activeBoxIds[activeBoxIdCount++] = BOXHEADADJ; } +#endif #ifdef GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 27ed999aef..a34e1666c6 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -74,7 +74,7 @@ #include "config/config_master.h" #ifdef USE_BST -void taskBstMasterProcess(uint32_t currentTime); +void taskBstMasterProcess(timeUs_t currentTimeUs); #endif #define TASK_PERIOD_HZ(hz) (1000000 / (hz)) @@ -87,16 +87,16 @@ void taskBstMasterProcess(uint32_t currentTime); #define IBATINTERVAL (6 * 3500) -static void taskUpdateAccelerometer(uint32_t currentTime) +static void taskUpdateAccelerometer(timeUs_t currentTimeUs) { - UNUSED(currentTime); + UNUSED(currentTimeUs); imuUpdateAccelerometer(&masterConfig.accelerometerTrims); } -static void taskHandleSerial(uint32_t currentTime) +static void taskHandleSerial(timeUs_t currentTimeUs) { - UNUSED(currentTime); + UNUSED(currentTimeUs); #ifdef USE_CLI // in cli mode, all serial stuff goes to here. enter cli mode by sending # if (cliMode) { @@ -107,13 +107,13 @@ static void taskHandleSerial(uint32_t currentTime) mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand); } -static void taskUpdateBattery(uint32_t currentTime) +static void taskUpdateBattery(timeUs_t currentTimeUs) { #ifdef USE_ADC static uint32_t vbatLastServiced = 0; if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_TELEMETRY)) { - if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) { - vbatLastServiced = currentTime; + if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) { + vbatLastServiced = currentTimeUs; updateBattery(); } } @@ -121,18 +121,18 @@ static void taskUpdateBattery(uint32_t currentTime) static uint32_t ibatLastServiced = 0; if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) { - const int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced); + const int32_t ibatTimeSinceLastServiced = cmp32(currentTimeUs, ibatLastServiced); if (ibatTimeSinceLastServiced >= IBATINTERVAL) { - ibatLastServiced = currentTime; + ibatLastServiced = currentTimeUs; updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); } } } -static void taskUpdateRxMain(uint32_t currentTime) +static void taskUpdateRxMain(timeUs_t currentTimeUs) { - processRx(currentTime); + processRx(currentTimeUs); isRXDataNew = true; #if !defined(BARO) && !defined(SONAR) @@ -155,18 +155,18 @@ static void taskUpdateRxMain(uint32_t currentTime) } #ifdef MAG -static void taskUpdateCompass(uint32_t currentTime) +static void taskUpdateCompass(timeUs_t currentTimeUs) { if (sensors(SENSOR_MAG)) { - compassUpdate(currentTime, &sensorTrims()->magZero); + compassUpdate(currentTimeUs, &sensorTrims()->magZero); } } #endif #ifdef BARO -static void taskUpdateBaro(uint32_t currentTime) +static void taskUpdateBaro(timeUs_t currentTimeUs) { - UNUSED(currentTime); + UNUSED(currentTimeUs); if (sensors(SENSOR_BARO)) { const uint32_t newDeadline = baroUpdate(); @@ -178,7 +178,7 @@ static void taskUpdateBaro(uint32_t currentTime) #endif #if defined(BARO) || defined(SONAR) -static void taskCalculateAltitude(uint32_t currentTime) +static void taskCalculateAltitude(timeUs_t currentTimeUs) { if (false #if defined(BARO) @@ -188,26 +188,26 @@ static void taskCalculateAltitude(uint32_t currentTime) || sensors(SENSOR_SONAR) #endif ) { - calculateEstimatedAltitude(currentTime); + calculateEstimatedAltitude(currentTimeUs); }} #endif #ifdef TELEMETRY -static void taskTelemetry(uint32_t currentTime) +static void taskTelemetry(timeUs_t currentTimeUs) { telemetryCheckState(); if (!cliMode && feature(FEATURE_TELEMETRY)) { - telemetryProcess(currentTime, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); } } #endif #ifdef USE_ESC_TELEMETRY -static void taskEscTelemetry(uint32_t currentTime) +static void taskEscTelemetry(timeUs_t currentTimeUs) { if (feature(FEATURE_ESC_TELEMETRY)) { - escTelemetryProcess(currentTime); + escTelemetryProcess(currentTimeUs); } } #endif diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 82e2d5423e..f6eaa56b97 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -493,12 +493,12 @@ void updateMagHold(void) magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } -void processRx(uint32_t currentTime) +void processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; static bool airmodeIsActivated; - calculateRxChannelsAndUpdateFailsafe(currentTime); + calculateRxChannelsAndUpdateFailsafe(currentTimeUs); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { @@ -506,11 +506,11 @@ void processRx(uint32_t currentTime) mwDisarm(); } - updateRSSI(currentTime); + updateRSSI(currentTimeUs); if (feature(FEATURE_FAILSAFE)) { - if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { + if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } @@ -811,9 +811,9 @@ uint8_t setPidUpdateCountDown(void) } // Function for loop trigger -void taskMainPidLoop(uint32_t currentTime) +void taskMainPidLoop(timeUs_t currentTimeUs) { - UNUSED(currentTime); + UNUSED(currentTimeUs); static bool runTaskMainSubprocesses; static uint8_t pidUpdateCountdown; diff --git a/src/main/fc/mw.h b/src/main/fc/mw.h index 3f6970da20..27c5680249 100644 --- a/src/main/fc/mw.h +++ b/src/main/fc/mw.h @@ -17,6 +17,8 @@ #pragma once +#include "common/time.h" + extern int16_t magHold; extern bool isRXDataNew; @@ -27,8 +29,8 @@ void handleInflightCalibrationStickPosition(); void mwDisarm(void); void mwArm(void); -void processRx(uint32_t currentTime); +void processRx(timeUs_t currentTimeUs); void updateLEDs(void); void updateRcCommands(void); -void taskMainPidLoop(uint32_t currentTime); +void taskMainPidLoop(timeUs_t currentTimeUs); diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index a04bd00ac8..9689b24b69 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -204,9 +204,9 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa return result; } -void calculateEstimatedAltitude(uint32_t currentTime) +void calculateEstimatedAltitude(timeUs_t currentTimeUs) { - static uint32_t previousTime; + static timeUs_t previousTimeUs; uint32_t dTime; int32_t baroVel; float dt; @@ -224,11 +224,11 @@ void calculateEstimatedAltitude(uint32_t currentTime) float sonarTransition; #endif - dTime = currentTime - previousTime; + dTime = currentTimeUs - previousTimeUs; if (dTime < BARO_UPDATE_FREQUENCY_40HZ) return; - previousTime = currentTime; + previousTimeUs = currentTimeUs; #ifdef BARO if (!isBaroCalibrationComplete()) { diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index b8d369b6db..3201f71b9a 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -20,7 +20,7 @@ extern int32_t AltHold; extern int32_t vario; -void calculateEstimatedAltitude(uint32_t currentTime); +void calculateEstimatedAltitude(timeUs_t currentTimeUs); struct pidProfile_s; struct barometerConfig_s; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d6c898c009..0ba4467071 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -369,7 +369,7 @@ static bool isMagnetometerHealthy(void) return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0); } -static void imuCalculateEstimatedAttitude(uint32_t currentTime) +static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) { static uint32_t previousIMUUpdateTime; float rawYawError = 0; @@ -377,8 +377,8 @@ static void imuCalculateEstimatedAttitude(uint32_t currentTime) bool useMag = false; bool useYaw = false; - uint32_t deltaT = currentTime - previousIMUUpdateTime; - previousIMUUpdateTime = currentTime; + uint32_t deltaT = currentTimeUs - previousIMUUpdateTime; + previousIMUUpdateTime = currentTimeUs; if (imuIsAccelerometerHealthy()) { useAcc = true; @@ -414,10 +414,10 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims) } } -void imuUpdateAttitude(uint32_t currentTime) +void imuUpdateAttitude(timeUs_t currentTimeUs) { if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) { - imuCalculateEstimatedAttitude(currentTime); + imuCalculateEstimatedAttitude(currentTimeUs); } else { acc.accSmooth[X] = 0; acc.accSmooth[Y] = 0; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 08b18a7d09..36439ea6df 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -19,6 +19,7 @@ #include "common/axis.h" #include "common/maths.h" +#include "common/time.h" #include "sensors/acceleration.h" @@ -96,10 +97,10 @@ void imuConfigure( ); float getCosTiltAngle(void); -void calculateEstimatedAltitude(uint32_t currentTime); +void calculateEstimatedAltitude(timeUs_t currentTimeUs); union rollAndPitchTrims_u; void imuUpdateAccelerometer(union rollAndPitchTrims_u *accelerometerTrims); -void imuUpdateAttitude(uint32_t currentTime); +void imuUpdateAttitude(timeUs_t currentTimeUs); float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz); diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 144cae3968..dffd456893 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -26,8 +26,9 @@ #include "build/debug.h" -#include "common/maths.h" #include "common/axis.h" +#include "common/maths.h" +#include "common/time.h" #include "drivers/system.h" diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 5ddd3c3ea8..13eefa691a 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -135,7 +135,7 @@ static uint32_t beeperNextToggleTime = 0; // Time of last arming beep in microseconds (for blackbox) static uint32_t armingBeepTimeMicros = 0; -static void beeperProcessCommand(uint32_t currentTime); +static void beeperProcessCommand(timeUs_t currentTimeUs); typedef struct beeperTableEntry_s { uint8_t mode; @@ -280,7 +280,7 @@ void beeperGpsStatus(void) * Beeper handler function to be called periodically in loop. Updates beeper * state via time schedule. */ -void beeperUpdate(uint32_t currentTime) +void beeperUpdate(timeUs_t currentTimeUs) { // If beeper option from AUX switch has been selected if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) { @@ -300,7 +300,7 @@ void beeperUpdate(uint32_t currentTime) return; } - if (beeperNextToggleTime > currentTime) { + if (beeperNextToggleTime > currentTimeUs) { return; } @@ -328,13 +328,13 @@ void beeperUpdate(uint32_t currentTime) } } - beeperProcessCommand(currentTime); + beeperProcessCommand(currentTimeUs); } /* * Calculates array position when next to change beeper state is due. */ -static void beeperProcessCommand(uint32_t currentTime) +static void beeperProcessCommand(timeUs_t currentTimeUs) { if (currentBeeperEntry->sequence[beeperPos] == BEEPER_COMMAND_REPEAT) { beeperPos = 0; @@ -342,7 +342,7 @@ static void beeperProcessCommand(uint32_t currentTime) beeperSilence(); } else { // Otherwise advance the sequence and calculate next toggle time - beeperNextToggleTime = currentTime + 1000 * 10 * currentBeeperEntry->sequence[beeperPos]; + beeperNextToggleTime = currentTimeUs + 1000 * 10 * currentBeeperEntry->sequence[beeperPos]; beeperPos++; } } @@ -400,7 +400,7 @@ bool isBeeperOn(void) void beeper(beeperMode_e mode) {UNUSED(mode);} void beeperSilence(void) {} void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);} -void beeperUpdate(uint32_t currentTime) {UNUSED(currentTime);} +void beeperUpdate(timeUs_t currentTimeUs) {UNUSED(currentTimeUs);} uint32_t getArmingBeepTimeMicros(void) {return 0;} beeperMode_e beeperModeForTableIndex(int idx) {UNUSED(idx); return BEEPER_SILENCE;} const char *beeperNameForTableIndex(int idx) {UNUSED(idx); return NULL;} diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index b89932d420..0822b285bb 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -17,6 +17,8 @@ #pragma once +#include "common/time.h" + typedef enum { // IMPORTANT: these are in priority order, 0 = Highest BEEPER_SILENCE = 0, // Silence, see beeperSilence() @@ -47,7 +49,7 @@ typedef enum { void beeper(beeperMode_e mode); void beeperSilence(void); -void beeperUpdate(uint32_t currentTime); +void beeperUpdate(timeUs_t currentTimeUs); void beeperConfirmationBeeps(uint8_t beepCount); uint32_t getArmingBeepTimeMicros(void); beeperMode_e beeperModeForTableIndex(int idx); diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 11bdf2ef1e..72819f28d0 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -586,7 +586,7 @@ void showDebugPage(void) } #endif -void dashboardUpdate(uint32_t currentTime) +void dashboardUpdate(timeUs_t currentTimeUs) { static uint8_t previousArmedState = 0; @@ -596,12 +596,12 @@ void dashboardUpdate(uint32_t currentTime) } #endif - const bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L; + const bool updateNow = (int32_t)(currentTimeUs - nextDisplayUpdateAt) >= 0L; if (!updateNow) { return; } - nextDisplayUpdateAt = currentTime + DISPLAY_UPDATE_FREQUENCY; + nextDisplayUpdateAt = currentTimeUs + DISPLAY_UPDATE_FREQUENCY; bool armedState = ARMING_FLAG(ARMED) ? true : false; bool armedStateChanged = armedState != previousArmedState; @@ -621,7 +621,7 @@ void dashboardUpdate(uint32_t currentTime) } pageState.pageChanging = (pageState.pageFlags & PAGE_STATE_FLAG_FORCE_PAGE_CHANGE) || - (((int32_t)(currentTime - pageState.nextPageAt) >= 0L && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED))); + (((int32_t)(currentTimeUs - pageState.nextPageAt) >= 0L && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED))); if (pageState.pageChanging && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)) { pageState.cycleIndex++; pageState.cycleIndex = pageState.cycleIndex % CYCLE_PAGE_ID_COUNT; @@ -631,7 +631,7 @@ void dashboardUpdate(uint32_t currentTime) if (pageState.pageChanging) { pageState.pageFlags &= ~PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; - pageState.nextPageAt = currentTime + PAGE_CYCLE_FREQUENCY; + pageState.nextPageAt = currentTimeUs + PAGE_CYCLE_FREQUENCY; // Some OLED displays do not respond on the first initialisation so refresh the display // when the page changes in the hopes the hardware responds. This also allows the @@ -730,7 +730,7 @@ void dashboardShowFixedPage(pageId_e pageId) dashboardDisablePageCycling(); } -void dashboardSetNextPageChangeAt(uint32_t futureMicros) +void dashboardSetNextPageChangeAt(timeUs_t futureMicros) { pageState.nextPageAt = futureMicros; } diff --git a/src/main/io/dashboard.h b/src/main/io/dashboard.h index cca02effdf..571de11973 100644 --- a/src/main/io/dashboard.h +++ b/src/main/io/dashboard.h @@ -15,6 +15,8 @@ * along with Cleanflight. If not, see . */ +#include "common/time.h" + #define ENABLE_DEBUG_DASHBOARD_PAGE typedef enum { @@ -37,11 +39,11 @@ typedef enum { struct rxConfig_s; void dashboardInit(struct rxConfig_s *intialRxConfig); -void dashboardUpdate(uint32_t currentTime); +void dashboardUpdate(timeUs_t currentTimeUs); void dashboardShowFixedPage(pageId_e pageId); void dashboardEnablePageCycling(void); void dashboardDisablePageCycling(void); void dashboardResetPageCycling(void); -void dashboardSetNextPageChangeAt(uint32_t futureMicros); +void dashboardSetNextPageChangeAt(timeUs_t futureMicros); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 2cd9873baf..15a6767beb 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -395,16 +395,16 @@ void gpsInitHardware(void) } } -static void updateGpsIndicator(uint32_t currentTime) +static void updateGpsIndicator(timeUs_t currentTimeUs) { static uint32_t GPSLEDTime; - if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) { - GPSLEDTime = currentTime + 150000; + if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) { + GPSLEDTime = currentTimeUs + 150000; LED1_TOGGLE; } } -void gpsUpdate(uint32_t currentTime) +void gpsUpdate(timeUs_t currentTimeUs) { // read out available GPS bytes if (gpsPort) { @@ -429,7 +429,7 @@ void gpsUpdate(uint32_t currentTime) gpsData.baudrateIndex++; gpsData.baudrateIndex %= GPS_INIT_ENTRIES; } - gpsData.lastMessage = currentTime / 1000; + gpsData.lastMessage = currentTimeUs / 1000; // TODO - move some / all of these into gpsData GPS_numSat = 0; DISABLE_STATE(GPS_FIX); @@ -438,7 +438,7 @@ void gpsUpdate(uint32_t currentTime) case GPS_RECEIVING_DATA: // check for no data/gps timeout/cable disconnection etc - if (currentTime / 1000 - gpsData.lastMessage > GPS_TIMEOUT) { + if (currentTimeUs / 1000 - gpsData.lastMessage > GPS_TIMEOUT) { // remove GPS from capability sensorsClear(SENSOR_GPS); gpsSetState(GPS_LOST_COMMUNICATION); @@ -446,7 +446,7 @@ void gpsUpdate(uint32_t currentTime) break; } if (sensors(SENSOR_GPS)) { - updateGpsIndicator(currentTime); + updateGpsIndicator(currentTimeUs); } } diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 7663e52f2f..5e258f3a06 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -17,6 +17,8 @@ #pragma once +#include "common/time.h" + #define LAT 0 #define LON 1 @@ -119,5 +121,5 @@ extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Str struct serialConfig_s; void gpsInit(struct serialConfig_s *serialConfig, gpsConfig_t *initialGpsConfig); -void gpsUpdate(uint32_t currentTime); +void gpsUpdate(timeUs_t currentTimeUs); bool gpsNewFrame(uint8_t c); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index e6bad4d189..310986ae2a 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -518,7 +518,7 @@ typedef enum { WARNING_FAILSAFE, } warningFlags_e; -static void applyLedWarningLayer(bool updateNow, uint32_t *timer) +static void applyLedWarningLayer(bool updateNow, timeUs_t *timer) { static uint8_t warningFlashCounter = 0; static uint8_t warningFlags = 0; // non-zero during blinks @@ -564,7 +564,7 @@ static void applyLedWarningLayer(bool updateNow, uint32_t *timer) } } -static void applyLedBatteryLayer(bool updateNow, uint32_t *timer) +static void applyLedBatteryLayer(bool updateNow, timeUs_t *timer) { static bool flash = false; @@ -601,7 +601,7 @@ static void applyLedBatteryLayer(bool updateNow, uint32_t *timer) } } -static void applyLedRssiLayer(bool updateNow, uint32_t *timer) +static void applyLedRssiLayer(bool updateNow, timeUs_t *timer) { static bool flash = false; @@ -632,7 +632,7 @@ static void applyLedRssiLayer(bool updateNow, uint32_t *timer) } #ifdef GPS -static void applyLedGpsLayer(bool updateNow, uint32_t *timer) +static void applyLedGpsLayer(bool updateNow, timeUs_t *timer) { static uint8_t gpsFlashCounter = 0; static uint8_t gpsPauseCounter = 0; @@ -671,7 +671,7 @@ static void applyLedGpsLayer(bool updateNow, uint32_t *timer) #define INDICATOR_DEADBAND 25 -static void applyLedIndicatorLayer(bool updateNow, uint32_t *timer) +static void applyLedIndicatorLayer(bool updateNow, timeUs_t *timer) { static bool flash = 0; @@ -734,7 +734,7 @@ static void updateLedRingCounts(void) ledCounts.ringSeqLen = seqLen; } -static void applyLedThrustRingLayer(bool updateNow, uint32_t *timer) +static void applyLedThrustRingLayer(bool updateNow, timeUs_t *timer) { static uint8_t rotationPhase; int ledRingIndex = 0; @@ -804,7 +804,7 @@ static void larsonScannerNextStep(larsonParameters_t *larsonParameters, int delt } } -static void applyLarsonScannerLayer(bool updateNow, uint32_t *timer) +static void applyLarsonScannerLayer(bool updateNow, timeUs_t *timer) { static larsonParameters_t larsonParameters = { 0, 0, 1 }; @@ -829,7 +829,7 @@ static void applyLarsonScannerLayer(bool updateNow, uint32_t *timer) } // blink twice, then wait ; either always or just when landing -static void applyLedBlinkLayer(bool updateNow, uint32_t *timer) +static void applyLedBlinkLayer(bool updateNow, timeUs_t *timer) { const uint16_t blinkPattern = 0x8005; // 0b1000000000000101; static uint16_t blinkMask; @@ -856,7 +856,7 @@ static void applyLedBlinkLayer(bool updateNow, uint32_t *timer) } #ifdef USE_LED_ANIMATION -static void applyLedAnimationLayer(bool updateNow, uint32_t *timer) +static void applyLedAnimationLayer(bool updateNow, timeUs_t *timer) { static uint8_t frameCounter = 0; const int animationFrames = ledGridHeight; @@ -904,14 +904,14 @@ typedef enum { timTimerCount } timId_e; -static uint32_t timerVal[timTimerCount]; +static timeUs_t timerVal[timTimerCount]; // function to apply layer. // function must replan self using timer pointer // when updateNow is true (timer triggered), state must be updated first, // before calculating led state. Otherwise update started by different trigger // may modify LED state. -typedef void applyLayerFn_timed(bool updateNow, uint32_t *timer); +typedef void applyLayerFn_timed(bool updateNow, timeUs_t *timer); static applyLayerFn_timed* layerTable[] = { [timBlink] = &applyLedBlinkLayer, @@ -929,7 +929,7 @@ static applyLayerFn_timed* layerTable[] = { [timRing] = &applyLedThrustRingLayer }; -void ledStripUpdate(uint32_t currentTime) +void ledStripUpdate(timeUs_t currentTimeUs) { if (!(ledStripInitialised && isWS2811LedStripReady())) { return; @@ -944,13 +944,13 @@ void ledStripUpdate(uint32_t currentTime) } ledStripEnabled = true; - const uint32_t now = currentTime; + const uint32_t now = currentTimeUs; // test all led timers, setting corresponding bits uint32_t timActive = 0; for (timId_e timId = 0; timId < timTimerCount; timId++) { // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value. - int32_t delta = cmp32(now, timerVal[timId]); + const timeDelta_t delta = cmpTimeUs(now, timerVal[timId]); // max delay is limited to 5s if (delta < 0 && delta > -MAX_TIMER_DELAY) continue; // not ready yet diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index a417509075..b545a3a4ec 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -18,6 +18,7 @@ #pragma once #include "common/color.h" +#include "common/time.h" #include "drivers/io_types.h" #define LED_MAX_STRIP_LENGTH 32 @@ -178,7 +179,7 @@ void reevaluateLedConfig(void); void ledStripInit(ledStripConfig_t *ledStripConfig); void ledStripEnable(void); -void ledStripUpdate(uint32_t currentTime); +void ledStripUpdate(timeUs_t currentTimeUs); bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e0afc4e7b4..92d071de1f 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -581,7 +581,7 @@ static void osdArmMotors(void) osdResetStats(); } -static void osdRefresh(uint32_t currentTime) +static void osdRefresh(timeUs_t currentTimeUs) { static uint8_t lastSec = 0; uint8_t sec; @@ -598,7 +598,7 @@ static void osdRefresh(uint32_t currentTime) osdUpdateStats(); - sec = currentTime / 1000000; + sec = currentTimeUs / 1000000; if (ARMING_FLAG(ARMED) && sec != lastSec) { flyTime++; @@ -614,7 +614,7 @@ static void osdRefresh(uint32_t currentTime) return; } - blinkState = (currentTime / 200000) % 2; + blinkState = (currentTimeUs / 200000) % 2; #ifdef CMS if (!displayIsGrabbed(osdDisplayPort)) { @@ -623,7 +623,7 @@ static void osdRefresh(uint32_t currentTime) displayHeartbeat(osdDisplayPort); // heartbeat to stop Minim OSD going back into native mode #ifdef OSD_CALLS_CMS } else { - cmsUpdate(currentTime); + cmsUpdate(currentTimeUs); #endif } #endif @@ -632,7 +632,7 @@ static void osdRefresh(uint32_t currentTime) /* * Called periodically by the scheduler */ -void osdUpdate(uint32_t currentTime) +void osdUpdate(timeUs_t currentTimeUs) { static uint32_t counter = 0; #ifdef MAX7456_DMA_CHANNEL_TX @@ -649,7 +649,7 @@ void osdUpdate(uint32_t currentTime) #define DRAW_FREQ_DENOM 10 // MWOSD @ 115200 baud #endif if (counter++ % DRAW_FREQ_DENOM == 0) { - osdRefresh(currentTime); + osdRefresh(currentTimeUs); } else { // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle displayDrawScreen(osdDisplayPort); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 8bdc878bd8..5a6a28ed1e 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -64,4 +64,4 @@ struct displayPort_s; void osdInit(struct displayPort_s *osdDisplayPort); void osdResetConfig(osd_profile_t *osdProfile); void osdResetAlarms(void); -void osdUpdate(uint32_t currentTime); +void osdUpdate(timeUs_t currentTimeUs); diff --git a/src/main/io/transponder_ir.c b/src/main/io/transponder_ir.c index 5ca4ba23e7..f378838b61 100644 --- a/src/main/io/transponder_ir.c +++ b/src/main/io/transponder_ir.c @@ -37,12 +37,12 @@ static bool transponderInitialised = false; static bool transponderRepeat = false; // timers -static uint32_t nextUpdateAt = 0; +static timeUs_t nextUpdateAtUs = 0; #define JITTER_DURATION_COUNT (sizeof(jitterDurations) / sizeof(uint8_t)) static uint8_t jitterDurations[] = {0,9,4,8,3,9,6,7,1,6,9,7,8,2,6}; -void transponderUpdate(uint32_t currentTime) +void transponderUpdate(timeUs_t currentTimeUs) { static uint32_t jitterIndex = 0; @@ -50,7 +50,7 @@ void transponderUpdate(uint32_t currentTime) return; } - const bool updateNow = (int32_t)(currentTime - nextUpdateAt) >= 0L; + const bool updateNow = (timeDelta_t)(currentTimeUs - nextUpdateAtUs) >= 0L; if (!updateNow) { return; } @@ -61,12 +61,12 @@ void transponderUpdate(uint32_t currentTime) jitterIndex = 0; } - nextUpdateAt = currentTime + 4500 + jitter; + nextUpdateAtUs = currentTimeUs + 4500 + jitter; #ifdef REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT // reduce current draw when USB cable is plugged in by decreasing the transponder transmit rate. if (usbCableIsInserted()) { - nextUpdateAt = currentTime + (1000 * 1000) / 10; // 10 hz. + nextUpdateAtUs = currentTimeUs + (1000 * 1000) / 10; // 10 hz. } #endif diff --git a/src/main/io/transponder_ir.h b/src/main/io/transponder_ir.h index 18a95f837a..27e48ce8af 100644 --- a/src/main/io/transponder_ir.h +++ b/src/main/io/transponder_ir.h @@ -17,11 +17,13 @@ #pragma once +#include "common/time.h" + void transponderInit(uint8_t* transponderCode); void transponderEnable(void); void transponderDisable(void); -void transponderUpdate(uint32_t currentTime); +void transponderUpdate(timeUs_t currentTimeUs); void transponderUpdateData(uint8_t* transponderData); void transponderTransmitOnce(void); void transponderStartRepeating(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index cbaea21b30..5a00e2e56a 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -306,12 +306,12 @@ void resumeRxSignal(void) failsafeOnRxResume(); } -bool rxUpdateCheck(uint32_t currentTime, uint32_t currentDeltaTime) +bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) { UNUSED(currentDeltaTime); if (rxSignalReceived) { - if (currentTime >= needRxSignalBefore) { + if (currentTimeUs >= needRxSignalBefore) { rxSignalReceived = false; rxSignalReceivedNotDataDriven = false; } @@ -322,14 +322,14 @@ bool rxUpdateCheck(uint32_t currentTime, uint32_t currentDeltaTime) if (isPPMDataBeingReceived()) { rxSignalReceivedNotDataDriven = true; rxIsInFailsafeModeNotDataDriven = false; - needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; + needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; resetPPMDataReceivedState(); } } else if (feature(FEATURE_RX_PARALLEL_PWM)) { if (isPWMDataBeingReceived()) { rxSignalReceivedNotDataDriven = true; rxIsInFailsafeModeNotDataDriven = false; - needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; + needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; } } else #endif @@ -340,10 +340,10 @@ bool rxUpdateCheck(uint32_t currentTime, uint32_t currentDeltaTime) rxDataReceived = true; rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0; rxSignalReceived = !rxIsInFailsafeMode; - needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; + needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; } } - return rxDataReceived || (currentTime >= rxUpdateAt); // data driven or 50Hz + return rxDataReceived || (currentTimeUs >= rxUpdateAt); // data driven or 50Hz } static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample) @@ -448,11 +448,11 @@ static void readRxChannelsApplyRanges(void) } } -static void detectAndApplySignalLossBehaviour(uint32_t currentTime) +static void detectAndApplySignalLossBehaviour(timeUs_t currentTimeUs) { bool useValueFromRx = true; const bool rxIsDataDriven = isRxDataDriven(); - const uint32_t currentMilliTime = currentTime / 1000; + const uint32_t currentMilliTime = currentTimeUs / 1000; if (!rxIsDataDriven) { rxSignalReceived = rxSignalReceivedNotDataDriven; @@ -513,20 +513,20 @@ static void detectAndApplySignalLossBehaviour(uint32_t currentTime) #endif } -void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime) +void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) { - rxUpdateAt = currentTime + DELAY_50_HZ; + rxUpdateAt = currentTimeUs + DELAY_50_HZ; // only proceed when no more samples to skip and suspend period is over if (skipRxSamples) { - if (currentTime > suspendRxSignalUntil) { + if (currentTimeUs > suspendRxSignalUntil) { skipRxSamples--; } return; } readRxChannelsApplyRanges(); - detectAndApplySignalLossBehaviour(currentTime); + detectAndApplySignalLossBehaviour(currentTimeUs); rcSampleIndex++; } @@ -560,19 +560,19 @@ static void updateRSSIPWM(void) #define RSSI_ADC_SAMPLE_COUNT 16 //#define RSSI_SCALE (0xFFF / 100.0f) -static void updateRSSIADC(uint32_t currentTime) +static void updateRSSIADC(timeUs_t currentTimeUs) { #ifndef USE_ADC - UNUSED(currentTime); + UNUSED(currentTimeUs); #else static uint8_t adcRssiSamples[RSSI_ADC_SAMPLE_COUNT]; static uint8_t adcRssiSampleIndex = 0; static uint32_t rssiUpdateAt = 0; - if ((int32_t)(currentTime - rssiUpdateAt) < 0) { + if ((int32_t)(currentTimeUs - rssiUpdateAt) < 0) { return; } - rssiUpdateAt = currentTime + DELAY_50_HZ; + rssiUpdateAt = currentTimeUs + DELAY_50_HZ; int16_t adcRssiMean = 0; uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); @@ -594,13 +594,13 @@ static void updateRSSIADC(uint32_t currentTime) #endif } -void updateRSSI(uint32_t currentTime) +void updateRSSI(timeUs_t currentTimeUs) { if (rxConfig->rssi_channel > 0) { updateRSSIPWM(); } else if (feature(FEATURE_RSSI_ADC)) { - updateRSSIADC(currentTime); + updateRSSIADC(currentTimeUs); } } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 3a47af66d2..7b881fd14e 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -17,6 +17,8 @@ #pragma once +#include "common/time.h" + #define STICK_CHANNEL_COUNT 4 #define PWM_RANGE_ZERO 0 // FIXME should all usages of this be changed to use PWM_RANGE_MIN? @@ -155,14 +157,14 @@ extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only need struct modeActivationCondition_s; void rxInit(const rxConfig_t *rxConfig, const struct modeActivationCondition_s *modeActivationConditions); void useRxConfig(const rxConfig_t *rxConfigToUse); -bool rxUpdateCheck(uint32_t currentTime, uint32_t currentDeltaTime); +bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); -void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime); +void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); void parseRcChannels(const char *input, rxConfig_t *rxConfig); -void updateRSSI(uint32_t currentTime); +void updateRSSI(timeUs_t currentTimeUs); void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChannelRangeConfiguration); void suspendRxSignal(void); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index d1bdb4a3ed..36f5559eff 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -113,7 +113,7 @@ typedef struct { /* Configuration */ const char * taskName; const char * subTaskName; - bool (*checkFunc)(timeUs_t currentTimeUs, uint32_t currentDeltaTimeUs); + bool (*checkFunc)(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); void (*taskFunc)(timeUs_t currentTimeUs); uint32_t desiredPeriod; // target period of execution const uint8_t staticPriority; // dynamicPriority grows in steps of this size, shouldn't be zero diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index d9b82932dd..e8c78c6087 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -88,9 +88,9 @@ static int32_t applySonarMedianFilter(int32_t newSonarReading) return newSonarReading; } -void sonarUpdate(uint32_t currentTime) +void sonarUpdate(timeUs_t currentTimeUs) { - UNUSED(currentTime); + UNUSED(currentTimeUs); hcsr04_start_reading(); } diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index f86feb04b4..16151fc2f9 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -17,9 +17,12 @@ #pragma once -#include "sensors/battery.h" +#include "common/time.h" + #include "drivers/sonar_hcsr04.h" +#include "sensors/battery.h" + #define SONAR_OUT_OF_RANGE (-1) extern int16_t sonarMaxRangeCm; @@ -27,7 +30,7 @@ extern int16_t sonarCfAltCm; extern int16_t sonarMaxAltWithTiltCm; void sonarInit(const sonarConfig_t *sonarConfig); -void sonarUpdate(uint32_t currentTime); +void sonarUpdate(timeUs_t currentTimeUs); int32_t sonarRead(void); int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle); int32_t sonarGetLatestAltitude(void); diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 216d9e486f..e4089f74a2 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -77,7 +77,7 @@ void targetConfiguration(master_t *config) config->rxConfig.sbus_inversion = 0; config->serialConfig.portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY; config->telemetryConfig.telemetry_inversion = 0; - intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT, &config->enabledFeatures); + intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY, &config->enabledFeatures); } config->profile[0].pidProfile.P8[ROLL] = 53; diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 889ebe09e0..bb9f92918f 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -1482,10 +1482,10 @@ void bstProcessInCommand(void) } } -static void resetBstChecker(uint32_t currentTime) +static void resetBstChecker(timeUs_t currentTimeUs) { if(needResetCheck) { - if(currentTime >= (resetBstTimer + BST_RESET_TIME)) + if(currentTimeUs >= (resetBstTimer + BST_RESET_TIME)) { bstTimeoutUserCallback(); needResetCheck = false; @@ -1502,14 +1502,14 @@ static uint32_t next20hzUpdateAt_1 = 0; static uint8_t sendCounter = 0; -void taskBstMasterProcess(uint32_t currentTime) +void taskBstMasterProcess(timeUs_t currentTimeUs) { if(coreProReady) { - if(currentTime >= next02hzUpdateAt_1 && !bstWriteBusy()) { + if(currentTimeUs >= next02hzUpdateAt_1 && !bstWriteBusy()) { writeFCModeToBST(); - next02hzUpdateAt_1 = currentTime + UPDATE_AT_02HZ; + next02hzUpdateAt_1 = currentTimeUs + UPDATE_AT_02HZ; } - if(currentTime >= next20hzUpdateAt_1 && !bstWriteBusy()) { + if(currentTimeUs >= next20hzUpdateAt_1 && !bstWriteBusy()) { if(sendCounter == 0) writeRCChannelToBST(); else if(sendCounter == 1) @@ -1517,7 +1517,7 @@ void taskBstMasterProcess(uint32_t currentTime) sendCounter++; if(sendCounter > 1) sendCounter = 0; - next20hzUpdateAt_1 = currentTime + UPDATE_AT_20HZ; + next20hzUpdateAt_1 = currentTimeUs + UPDATE_AT_20HZ; } if(sensors(SENSOR_GPS) && !bstWriteBusy()) @@ -1529,7 +1529,7 @@ void taskBstMasterProcess(uint32_t currentTime) stopMotors(); systemReset(); } - resetBstChecker(currentTime); + resetBstChecker(currentTimeUs); } /*************************************************************************************************/ diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.h b/src/main/target/COLIBRI_RACE/i2c_bst.h index 2c90388477..ea3ab556cd 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.h +++ b/src/main/target/COLIBRI_RACE/i2c_bst.h @@ -19,7 +19,7 @@ void bstProcessInCommand(void); void bstSlaveProcessInCommand(void); -void taskBstMasterProcess(uint32_t currentTime); +void taskBstMasterProcess(timeUs_t currentTimeUs); bool writeGpsPositionPrameToBST(void); bool writeRollPitchYawToBST(void); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 802d25a14a..2361e05fd8 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -330,7 +330,7 @@ bool checkCrsfTelemetryState(void) /* * Called periodically by the scheduler */ -void handleCrsfTelemetry(uint32_t currentTime) +void handleCrsfTelemetry(timeUs_t currentTimeUs) { static uint32_t crsfLastCycleTime; @@ -343,8 +343,8 @@ void handleCrsfTelemetry(uint32_t currentTime) crsfRxSendTelemetryData(); // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz - if (currentTime >= crsfLastCycleTime + CRSF_CYCLETIME_US) { - crsfLastCycleTime = currentTime; + if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) { + crsfLastCycleTime = currentTimeUs; processCrsf(); } } diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index d6a126a29e..3685f512cc 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -19,6 +19,8 @@ #pragma once +#include "common/time.h" + typedef enum { CRSF_FRAME_START = 0, CRSF_FRAME_ATTITUDE = CRSF_FRAME_START, @@ -30,7 +32,7 @@ typedef enum { void initCrsfTelemetry(void); bool checkCrsfTelemetryState(void); -void handleCrsfTelemetry(uint32_t currentTime); +void handleCrsfTelemetry(timeUs_t currentTimeUs); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); diff --git a/src/main/telemetry/esc_telemetry.c b/src/main/telemetry/esc_telemetry.c index 7dd37ac08d..7404254eda 100644 --- a/src/main/telemetry/esc_telemetry.c +++ b/src/main/telemetry/esc_telemetry.c @@ -200,9 +200,9 @@ uint8_t escTelemetryFrameStatus(void) return frameStatus; } -void escTelemetryProcess(uint32_t currentTime) +void escTelemetryProcess(timeUs_t currentTimeUs) { - uint32_t currentTimeMs = currentTime / 1000; + const timeMs_t currentTimeMs = currentTimeUs / 1000; if (!escTelemetryEnabled) { return; diff --git a/src/main/telemetry/esc_telemetry.h b/src/main/telemetry/esc_telemetry.h index 6eb0c5f622..ca9c2a2274 100644 --- a/src/main/telemetry/esc_telemetry.h +++ b/src/main/telemetry/esc_telemetry.h @@ -1,5 +1,7 @@ #pragma once +#include "common/time.h" + uint8_t escTelemetryFrameStatus(void); bool escTelemetryInit(void); bool isEscTelemetryActive(void); @@ -7,4 +9,4 @@ int16_t getEscTelemetryVbat(void); int16_t getEscTelemetryCurrent(void); int16_t getEscTelemetryConsumption(void); -void escTelemetryProcess(uint32_t currentTime); +void escTelemetryProcess(timeUs_t currentTimeUs); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 997c1d3659..588b35dc47 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -489,33 +489,33 @@ void checkHoTTTelemetryState(void) freeHoTTTelemetryPort(); } -void handleHoTTTelemetry(uint32_t currentTime) +void handleHoTTTelemetry(timeUs_t currentTimeUs) { - static uint32_t serialTimer; + static timeUs_t serialTimer; if (!hottTelemetryEnabled) { return; } - if (shouldPrepareHoTTMessages(currentTime)) { + if (shouldPrepareHoTTMessages(currentTimeUs)) { hottPrepareMessages(); - lastMessagesPreparedAt = currentTime; + lastMessagesPreparedAt = currentTimeUs; } if (shouldCheckForHoTTRequest()) { - hottCheckSerialData(currentTime); + hottCheckSerialData(currentTimeUs); } if (!hottMsg) return; if (hottIsSending) { - if(currentTime - serialTimer < HOTT_TX_DELAY_US) { + if(currentTimeUs - serialTimer < HOTT_TX_DELAY_US) { return; } } hottSendTelemetryData(); - serialTimer = currentTime; + serialTimer = currentTimeUs; } #endif diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index f66a990d10..d01b45b34f 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -25,6 +25,8 @@ #pragma once +#include "common/time.h" + #define HOTTV4_RXTX 4 #define HOTTV4_TEXT_MODE_REQUEST_ID 0x7f @@ -485,7 +487,7 @@ typedef struct HOTT_AIRESC_MSG_s { uint8_t stop_byte; //#44 constant value 0x7d } HOTT_AIRESC_MSG_t; -void handleHoTTTelemetry(uint32_t currentTime); +void handleHoTTTelemetry(timeUs_t currentTimeUs); void checkHoTTTelemetryState(void); void initHoTTTelemetry(telemetryConfig_t *telemetryConfig);