Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +03:00
Code Issues Wiki Activity

Merge pull request #755 from martinbudden/inav_scheduler

Browse source 
Pass currentTime as a parameter to all task functions
This commit is contained in:
Konstantin Sharlaimov 2016-11-06 17:15:56 +10:00 • committed by GitHub
commit fff6a2e474
25 changed files with 123 additions and 140 deletions
Download patch file Download diff file Expand all files Collapse all files

25
src/main/blackbox/blackbox.c
View file

@ -373,9 +373,6 @@ typedef struct blackboxSlowState_s {
//From mixer.c:
extern uint8_t motorCount;
//From mw.c:
extern uint32_t currentTime;
//From rx.c:
extern uint16_t rssi;
@ -1024,7 +1021,7 @@ static void writeGPSHomeFrame()
gpsHistory.GPS_home[1] = GPS_home.lon;
}
static void writeGPSFrame()
static void writeGPSFrame(uint32_t currentTime)
{
blackboxWrite('G');
@ -1059,7 +1056,7 @@ static void writeGPSFrame()
/**
* Fill the current state of the blackbox using values read from the flight controller
*/
static void loadMainState(void)
static void loadMainState(uint32_t currentTime)
{
blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
int i;
@ -1442,7 +1439,7 @@ static void blackboxAdvanceIterationTimers()
}
// Called once every FC loop in order to log the current state
static void blackboxLogIteration()
static void blackboxLogIteration(uint32_t currentTime)
{
// Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
if (blackboxShouldLogIFrame()) {
@ -1452,7 +1449,7 @@ static void blackboxLogIteration()
*/
writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes());
loadMainState();
loadMainState(currentTime);
writeIntraframe();
} else {
blackboxCheckAndLogArmingBeep();
@ -1464,7 +1461,7 @@ static void blackboxLogIteration()
*/
writeSlowFrameIfNeeded(true);
loadMainState();
loadMainState(currentTime);
writeInterframe();
}
#ifdef GPS
@ -1479,12 +1476,12 @@ static void blackboxLogIteration()
if (GPS_home.lat != gpsHistory.GPS_home[0] || GPS_home.lon != gpsHistory.GPS_home[1]
|| (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
writeGPSHomeFrame();
writeGPSFrame();
writeGPSHomeFrame(currentTime);
writeGPSFrame(currentTime);
} else if (gpsSol.numSat != gpsHistory.GPS_numSat || gpsSol.llh.lat != gpsHistory.GPS_coord[0]
|| gpsSol.llh.lon != gpsHistory.GPS_coord[1]) {
//We could check for velocity changes as well but I doubt it changes independent of position
writeGPSFrame();
writeGPSFrame(currentTime);
}
}
#endif
@ -1497,7 +1494,7 @@ static void blackboxLogIteration()
/**
* Call each flight loop iteration to perform blackbox logging.
*/
void handleBlackbox(void)
void handleBlackbox(uint32_t currentTime)
{
int i;
@ -1594,7 +1591,7 @@ void handleBlackbox(void)
blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
blackboxSetState(BLACKBOX_STATE_RUNNING);
blackboxLogIteration();
blackboxLogIteration(currentTime);
}
// Keep the logging timers ticking so our log iteration continues to advance
@ -1605,7 +1602,7 @@ void handleBlackbox(void)
if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
blackboxSetState(BLACKBOX_STATE_PAUSED);
} else {
blackboxLogIteration();
blackboxLogIteration(currentTime);
}
blackboxAdvanceIterationTimers();

2
src/main/blackbox/blackbox.h
View file

@ -22,7 +22,7 @@
void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
void initBlackbox(void);
void handleBlackbox(void);
void handleBlackbox(uint32_t currentTime);
void startBlackbox(void);
void finishBlackbox(void);
bool blackboxMayEditConfig(void);

51
src/main/fc/fc_tasks.c
View file

@ -79,15 +79,14 @@
#include "config/config_profile.h"
#include "config/config_master.h"
extern uint32_t currentTime;
/* VBAT monitoring interval (in microseconds) - 1s*/
#define VBATINTERVAL (6 * 3500)
/* IBat monitoring interval (in microseconds) - 6 default looptimes */
#define IBATINTERVAL (6 * 3500)
void taskHandleSerial(void)
void taskHandleSerial(uint32_t currentTime)
{
UNUSED(currentTime);
#ifdef USE_CLI
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
@ -98,12 +97,12 @@ void taskHandleSerial(void)
mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA);
}
void taskUpdateBeeper(void)
void taskUpdateBeeper(uint32_t currentTime)
{
beeperUpdate(); //call periodic beeper handler
beeperUpdate(currentTime); //call periodic beeper handler
}
void taskUpdateBattery(void)
void taskUpdateBattery(uint32_t currentTime)
{
static uint32_t vbatLastServiced = 0;
static uint32_t ibatLastServiced = 0;
@ -127,7 +126,7 @@ void taskUpdateBattery(void)
}
#ifdef GPS
void taskProcessGPS(void)
void taskProcessGPS(uint32_t currentTime)
{
// if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
// hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
@ -143,17 +142,19 @@ void taskProcessGPS(void)
#endif
#ifdef MAG
void taskUpdateCompass(void)
void taskUpdateCompass(uint32_t currentTime)
{
if (sensors(SENSOR_MAG)) {
updateCompass(&masterConfig.magZero);
updateCompass(currentTime, &masterConfig.magZero);
}
}
#endif
#ifdef BARO
void taskUpdateBaro(void)
void taskUpdateBaro(uint32_t currentTime)
{
UNUSED(currentTime);
if (sensors(SENSOR_BARO)) {
const uint32_t newDeadline = baroUpdate();
if (newDeadline != 0) {
@ -166,8 +167,10 @@ void taskUpdateBaro(void)
#endif
#ifdef SONAR
void taskUpdateSonar(void)
void taskUpdateSonar(uint32_t currentTime)
{
UNUSED(currentTime);
if (sensors(SENSOR_SONAR)) {
rangefinderUpdate();
}
@ -177,36 +180,38 @@ void taskUpdateSonar(void)
#endif
#ifdef DISPLAY
void taskUpdateDisplay(void)
void taskUpdateDisplay(uint32_t currentTime)
{
if (feature(FEATURE_DISPLAY)) {
updateDisplay();
updateDisplay(currentTime);
}
}
#endif
#ifdef TELEMETRY
void taskTelemetry(void)
void taskTelemetry(uint32_t currentTime)
{
telemetryCheckState();
if (!cliMode && feature(FEATURE_TELEMETRY)) {
telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
telemetryProcess(currentTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
}
#endif
#ifdef LED_STRIP
void taskLedStrip(void)
void taskLedStrip(uint32_t currentTime)
{
if (feature(FEATURE_LED_STRIP)) {
updateLedStrip();
updateLedStrip(currentTime);
}
}
#endif
#ifdef USE_PMW_SERVO_DRIVER
void taskSyncPwmDriver(void) {
void taskSyncPwmDriver(uint32_t currentTime)
{
UNUSED(currentTime);
if (feature(FEATURE_PWM_SERVO_DRIVER)) {
pwmDriverSync();
@ -215,11 +220,15 @@ void taskSyncPwmDriver(void) {
#endif
#ifdef ASYNC_GYRO_PROCESSING
void taskAttitude(void) {
imuUpdateAttitude();
void taskAttitude(uint32_t currentTime)
{
imuUpdateAttitude(currentTime);
}
void taskAcc(void) {
void taskAcc(uint32_t currentTime)
{
UNUSED(currentTime);
imuUpdateAccelerometer();
}
#endif

26
src/main/fc/fc_tasks.h
View file

@ -19,25 +19,13 @@
#include <stdint.h>
void taskHandleSerial(void);
void taskUpdateBeeper(void);
void taskUpdateBattery(void);
bool taskUpdateRxCheck(uint32_t currentDeltaTime);
void taskUpdateRxMain(void);
void taskSystem(void);
void taskMainPidLoopChecker(uint32_t currentTime);
bool taskUpdateRxCheck(uint32_t currentTime, uint32_t currentDeltaTime);
void taskUpdateRxMain(uint32_t currentTime);
void taskSystem(uint32_t currentTime);
void taskStackCheck(uint32_t currentTime);
#ifdef USE_PMW_SERVO_DRIVER
void taskSyncPwmDriver(void);
#endif
void taskStackCheck(void);
void taskMainPidLoop(void);
void taskGyro(void);
#ifdef ASYNC_GYRO_PROCESSING
void taskAcc(void);
void taskAttitude(void);
#endif
void taskMainPidLoop(uint32_t currentTime);
void taskGyro(uint32_t currentTime);
void fcTasksInit(void);

32
src/main/fc/mw.c
View file

@ -100,8 +100,6 @@ uint8_t motorControlEnable = false;
int16_t telemTemperature1; // gyro sensor temperature
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
extern uint32_t currentTime;
static bool isRXDataNew;
bool isCalibrating(void)
@ -258,7 +256,7 @@ void mwArm(void)
}
}
void processRx(void)
void processRx(uint32_t currentTime)
{
static bool armedBeeperOn = false;
@ -522,13 +520,13 @@ void filterRc(bool isRXDataNew)
}
// Function for loop trigger
void taskGyro(void) {
void taskGyro(uint32_t currentTime) {
// getTaskDeltaTime() returns delta time freezed at the moment of entering the scheduler. currentTime is freezed at the very same point.
// To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
const uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
if (masterConfig.gyroSync) {
while (1) {
while (true) {
#ifdef ASYNC_GYRO_PROCESSING
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
#else
@ -548,25 +546,25 @@ void taskGyro(void) {
#endif
}
void taskMainPidLoop(void)
void taskMainPidLoop(uint32_t currentTime)
{
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)cycleTime * 0.000001f;
#ifdef ASYNC_GYRO_PROCESSING
if (getAsyncMode() == ASYNC_MODE_NONE) {
taskGyro();
taskGyro(currentTime);
}
if (getAsyncMode() != ASYNC_MODE_ALL && sensors(SENSOR_ACC)) {
imuUpdateAccelerometer();
imuUpdateAttitude();
imuUpdateAttitude(currentTime);
}
#else
/* Update gyroscope */
taskGyro();
taskGyro(currentTime);
imuUpdateAccelerometer();
imuUpdateAttitude();
imuUpdateAttitude(currentTime);
#endif
@ -670,21 +668,21 @@ void taskMainPidLoop(void)
#ifdef BLACKBOX
if (!cliMode && feature(FEATURE_BLACKBOX)) {
handleBlackbox();
handleBlackbox(micros());
}
#endif
}
bool taskUpdateRxCheck(uint32_t currentDeltaTime)
bool taskUpdateRxCheck(uint32_t currentTime, uint32_t currentDeltaTime)
{
UNUSED(currentDeltaTime);
updateRx(currentTime);
return shouldProcessRx(currentTime);
return updateRx(currentTime);
}
void taskUpdateRxMain(void)
void taskUpdateRxMain(uint32_t currentTime)
{
processRx();
processRx(currentTime);
isRXDataNew = true;
}

5
src/main/flight/imu.c
View file

@ -574,12 +574,11 @@ void imuUpdateAccelerometer(void)
#endif
}
void imuUpdateAttitude(void)
void imuUpdateAttitude(uint32_t currentTime)
{
/* Calculate dT */
static uint32_t previousIMUUpdateTime;
uint32_t currentTime = micros();
float dT = (currentTime - previousIMUUpdateTime) * 1e-6;
const float dT = (currentTime - previousIMUUpdateTime) * 1e-6;
previousIMUUpdateTime = currentTime;
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {

2
src/main/flight/imu.h
View file

@ -49,7 +49,7 @@ typedef struct imuRuntimeConfig_s {
struct pidProfile_s;
void imuConfigure(imuRuntimeConfig_t *initialImuRuntimeConfig, struct pidProfile_s *initialPidProfile);
void imuUpdateAttitude(void);
void imuUpdateAttitude(uint32_t currentTime);
void imuUpdateAccelerometer(void);
void imuUpdateGyroscope(uint32_t gyroUpdateDeltaUs);
float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompensationStrength);

6
src/main/io/beeper.c
View file

@ -287,7 +287,7 @@ void beeperGpsStatus(void)
* Beeper handler function to be called periodically in loop. Updates beeper
* state via time schedule.
*/
void beeperUpdate(void)
void beeperUpdate(uint32_t currentTime)
{
// If beeper option from AUX switch has been selected
if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) {
@ -307,7 +307,7 @@ void beeperUpdate(void)
return;
}
uint32_t now = millis();
const uint32_t now = currentTime / 1000;
if (beeperNextToggleTime > now) {
return;
}
@ -325,7 +325,7 @@ void beeperUpdate(void)
beeperPos == 0
&& (currentBeeperEntry->mode == BEEPER_ARMING || currentBeeperEntry->mode == BEEPER_ARMING_GPS_FIX)
) {
armingBeepTimeMicros = micros();
armingBeepTimeMicros = currentTime;
}
}
} else {

2
src/main/io/beeper.h
View file

@ -48,7 +48,7 @@ typedef enum {
void beeper(beeperMode_e mode);
void beeperSilence(void);
void beeperUpdate(void);
void beeperUpdate(uint32_t currentTime);
void beeperConfirmationBeeps(uint8_t beepCount);
uint32_t getArmingBeepTimeMicros(void);
beeperMode_e beeperModeForTableIndex(int idx);

14
src/main/io/display.c
View file

@ -370,19 +370,18 @@ void showStatusPage(void)
}
void updateDisplay(void)
void updateDisplay(uint32_t currentTime)
{
uint32_t now = micros();
static uint8_t previousArmedState = 0;
bool pageChanging = false;
bool updateNow = (int32_t)(now - nextDisplayUpdateAt) >= 0L;
bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L;
if (!updateNow) {
return;
}
nextDisplayUpdateAt = now + DISPLAY_UPDATE_FREQUENCY;
nextDisplayUpdateAt = currentTime + DISPLAY_UPDATE_FREQUENCY;
bool armedState = ARMING_FLAG(ARMED) ? true : false;
bool armedStateChanged = armedState != previousArmedState;
@ -400,7 +399,7 @@ void updateDisplay(void)
pageChanging = true;
}
if ((currentPageId == PAGE_WELCOME) && ((int32_t)(now - nextPageAt) >= 0L)) {
if ((currentPageId == PAGE_WELCOME) && ((int32_t)(currentTime - nextPageAt) >= 0L)) {
currentPageId = PAGE_STATUS;
pageChanging = true;
}
@ -465,9 +464,10 @@ void displayInit(rxConfig_t *rxConfigToUse)
rxConfig = rxConfigToUse;
displaySetPage(PAGE_WELCOME);
displaySetNextPageChangeAt(micros() + (1000 * 1000 * 5));
const uint32_t now = micros();
displaySetNextPageChangeAt(now + 5* MICROSECONDS_IN_A_SECOND);
updateDisplay();
updateDisplay(now);
}
void displaySetNextPageChangeAt(uint32_t futureMicros)

2
src/main/io/display.h
View file

@ -23,7 +23,7 @@ typedef enum {
struct rxConfig_s;
void displayInit(struct rxConfig_s *intialRxConfig);
void updateDisplay(void);
void updateDisplay(uint32_t currentTime);
void displaySetPage(pageId_e newPageId);
void displaySetNextPageChangeAt(uint32_t futureMicros);

8
src/main/io/ledstrip.c
View file

@ -920,7 +920,7 @@ static applyLayerFn_timed* layerTable[] = {
[timRing] = &applyLedThrustRingLayer
};
void updateLedStrip(void)
void updateLedStrip(uint32_t currentTime)
{
if (!(ledStripInitialised && isWS2811LedStripReady())) {
return;
@ -935,19 +935,17 @@ void updateLedStrip(void)
}
ledStripEnabled = true;
uint32_t now = micros();
// 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.
// max delay is limited to 5s
int32_t delta = cmp32(now, timerVal[timId]);
int32_t delta = cmp32(currentTime, timerVal[timId]);
if (delta < 0 && delta > -LED_STRIP_MS(5000))
continue; // not ready yet
timActive |= 1 << timId;
if (delta >= LED_STRIP_MS(100) || delta < 0) {
timerVal[timId] = now;
timerVal[timId] = currentTime;
}
}

2
src/main/io/ledstrip.h
View file

@ -166,7 +166,7 @@ void reevaluateLedConfig(void);
void ledStripInit(ledConfig_t *ledConfigsToUse, struct hsvColor_s *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse);
void ledStripEnable(void);
void updateLedStrip(void);
void updateLedStrip(uint32_t currentTime);
bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex);

6
src/main/rx/rx.c
View file

@ -367,7 +367,7 @@ void resumeRxSignal(void)
failsafeOnRxResume();
}
void updateRx(uint32_t currentTime)
bool updateRx(uint32_t currentTime)
{
resetRxSignalReceivedFlagIfNeeded(currentTime);
@ -429,10 +429,6 @@ void updateRx(uint32_t currentTime)
}
}
#endif
}
bool shouldProcessRx(uint32_t currentTime)
{
return rxDataReceived || ((int32_t)(currentTime - rxUpdateAt) >= 0); // data driven or 50Hz
}

3
src/main/rx/rx.h
View file

@ -147,10 +147,9 @@ typedef uint16_t (*rcReadRawDataPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, u
struct modeActivationCondition_s;
void rxInit(rxConfig_t *rxConfig, struct modeActivationCondition_s *modeActivationConditions);
void useRxConfig(rxConfig_t *rxConfigToUse);
void updateRx(uint32_t currentTime);
bool updateRx(uint32_t currentTime);
bool rxIsReceivingSignal(void);
bool rxAreFlightChannelsValid(void);
bool shouldProcessRx(uint32_t currentTime);
void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime);
void parseRcChannels(const char *input, rxConfig_t *rxConfig);

16
src/main/scheduler/scheduler.c
View file

@ -40,7 +40,6 @@ static uint32_t totalWaitingTasks;
static uint32_t totalWaitingTasksSamples;
static uint32_t realtimeGuardInterval;
uint32_t currentTime = 0;
uint16_t averageSystemLoadPercent = 0;
#define REALTIME_GUARD_INTERVAL_MIN 10
@ -124,16 +123,18 @@ STATIC_INLINE_UNIT_TESTED cfTask_t *queueNext(void)
return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
}
void taskSystem(void)
void taskSystem(uint32_t currentTime)
{
/* Calculate system load */
UNUSED(currentTime);
// Calculate system load
if (totalWaitingTasksSamples > 0) {
averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples;
totalWaitingTasksSamples = 0;
totalWaitingTasks = 0;
}
/* Calculate guard interval */
// Calculate guard interval
uint32_t maxNonRealtimeTaskTime = 0;
for (const cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
if (task->staticPriority != TASK_PRIORITY_REALTIME) {
@ -200,8 +201,9 @@ void schedulerInit(void)
void scheduler(void)
{
// Cache currentTime
currentTime = micros();
const uint32_t currentTime = micros();
// Check for realtime tasks
uint32_t timeToNextRealtimeTask = UINT32_MAX;
for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) {
const uint32_t nextExecuteAt = task->lastExecutedAt + task->desiredPeriod;
@ -228,7 +230,7 @@ void scheduler(void)
task->taskAgeCycles = 1 + ((currentTime - task->lastSignaledAt) / task->desiredPeriod);
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
waitingTasks++;
} else if (task->checkFunc(currentTime - task->lastExecutedAt)) {
} else if (task->checkFunc(currentTime, currentTime - task->lastExecutedAt)) {
task->lastSignaledAt = currentTime;
task->taskAgeCycles = 1;
task->dynamicPriority = 1 + task->staticPriority;
@ -271,7 +273,7 @@ void scheduler(void)
// Execute task
const uint32_t currentTimeBeforeTaskCall = micros();
selectedTask->taskFunc();
selectedTask->taskFunc(currentTimeBeforeTaskCall);
const uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
selectedTask->averageExecutionTime = ((uint32_t)selectedTask->averageExecutionTime * 31 + taskExecutionTime) / 32;

4
src/main/scheduler/scheduler.h
View file

@ -93,8 +93,8 @@ typedef enum {
typedef struct {
/* Configuration */
const char * taskName;
bool (*checkFunc)(uint32_t currentDeltaTime);
void (*taskFunc)(void);
bool (*checkFunc)(uint32_t currentTime, uint32_t currentDeltaTime);
void (*taskFunc)(uint32_t currentTime);
uint32_t desiredPeriod; // target period of execution
const uint8_t staticPriority; // dynamicPriority grows in steps of this size, shouldn't be zero

4
src/main/sensors/compass.c
View file

@ -43,8 +43,6 @@
mag_t mag; // mag access functions
float magneticDeclination = 0.0f; // calculated at startup from config
extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
int16_t magADCRaw[XYZ_AXIS_COUNT];
int32_t magADC[XYZ_AXIS_COUNT];
sensor_align_e magAlign = 0;
@ -74,7 +72,7 @@ bool isCompassReady(void)
static sensorCalibrationState_t calState;
void updateCompass(flightDynamicsTrims_t *magZero)
void updateCompass(uint32_t currentTime, flightDynamicsTrims_t *magZero)
{
static uint32_t calStartedAt = 0;
static int16_t magPrev[XYZ_AXIS_COUNT];

4
src/main/sensors/compass.h
View file

@ -33,12 +33,10 @@ typedef enum {
#define MAG_MAX MAG_FAKE
#ifdef MAG
bool compassInit(int16_t magDeclinationFromConfig);
union flightDynamicsTrims_u;
void updateCompass(union flightDynamicsTrims_u *magZero);
void updateCompass(uint32_t currentTime, union flightDynamicsTrims_u *magZero);
bool isCompassReady(void);
#endif
extern int32_t magADC[XYZ_AXIS_COUNT];

14
src/main/telemetry/hott.c
View file

@ -492,7 +492,7 @@ void checkHoTTTelemetryState(void)
freeHoTTTelemetryPort();
}
void handleHoTTTelemetry(void)
void handleHoTTTelemetry(uint32_t currentTime)
{
static uint32_t serialTimer;
@ -500,27 +500,25 @@ void handleHoTTTelemetry(void)
return;
}
uint32_t now = micros();
if (shouldPrepareHoTTMessages(now)) {
if (shouldPrepareHoTTMessages(currentTime)) {
hottPrepareMessages();
lastMessagesPreparedAt = now;
lastMessagesPreparedAt = currentTime;
}
if (shouldCheckForHoTTRequest()) {
hottCheckSerialData(now);
hottCheckSerialData(currentTime);
}
if (!hottMsg)
return;
if (hottIsSending) {
if(now - serialTimer < HOTT_TX_DELAY_US) {
if(currentTime - serialTimer < HOTT_TX_DELAY_US) {
return;
}
}
hottSendTelemetryData();
serialTimer = now;
serialTimer = currentTime;
}
#endif

2
src/main/telemetry/hott.h
View file

@ -487,7 +487,7 @@ typedef struct HOTT_AIRESC_MSG_s {
uint8_t stop_byte; //#44 constant value 0x7d
} HOTT_AIRESC_MSG_t;
void handleHoTTTelemetry(void);
void handleHoTTTelemetry(uint32_t currentTime);
void checkHoTTTelemetryState(void);
void initHoTTTelemetry(telemetryConfig_t *telemetryConfig);

19
src/main/telemetry/mavlink.c
View file

@ -272,7 +272,7 @@ void mavlinkSendRCChannelsAndRSSI(void)
}
#if defined(GPS)
void mavlinkSendPosition(void)
void mavlinkSendPosition(uint32_t currentTime)
{
uint16_t msgLength;
uint8_t gpsFixType = 0;
@ -289,7 +289,7 @@ void mavlinkSendPosition(void)
mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg,
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
micros(),
currentTime,
// fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
gpsFixType,
// lat Latitude in 1E7 degrees
@ -314,7 +314,7 @@ void mavlinkSendPosition(void)
// Global position
mavlink_msg_global_position_int_pack(0, 200, &mavMsg,
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
micros(),
currentTime,
// lat Latitude in 1E7 degrees
gpsSol.llh.lat,
// lon Longitude in 1E7 degrees
@ -501,7 +501,7 @@ void mavlinkSendHUDAndHeartbeat(void)
mavlinkSerialWrite(mavBuffer, msgLength);
}
void processMAVLinkTelemetry(void)
void processMAVLinkTelemetry(uint32_t currentTime)
{
// is executed @ TELEMETRY_MAVLINK_MAXRATE rate
if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS)) {
@ -514,7 +514,7 @@ void processMAVLinkTelemetry(void)
#ifdef GPS
if (mavlinkStreamTrigger(MAV_DATA_STREAM_POSITION)) {
mavlinkSendPosition();
mavlinkSendPosition(currentTime);
}
#endif
@ -527,7 +527,7 @@ void processMAVLinkTelemetry(void)
}
}
void handleMAVLinkTelemetry(void)
void handleMAVLinkTelemetry(uint32_t currentTime)
{
if (!mavlinkTelemetryEnabled) {
return;
@ -537,10 +537,9 @@ void handleMAVLinkTelemetry(void)
return;
}
uint32_t now = micros();
if ((now - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
processMAVLinkTelemetry();
lastMavlinkMessage = now;
if ((currentTime - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
processMAVLinkTelemetry(currentTime);
lastMavlinkMessage = currentTime;
}
}

2
src/main/telemetry/mavlink.h
View file

@ -19,7 +19,7 @@
#define TELEMETRY_MAVLINK_H_
void initMAVLinkTelemetry(void);
void handleMAVLinkTelemetry(void);
void handleMAVLinkTelemetry(uint32_t currentTime);
void checkMAVLinkTelemetryState(void);
void freeMAVLinkTelemetryPort(void);

10
src/main/telemetry/telemetry.c
View file

@ -138,7 +138,7 @@ void telemetryCheckState(void)
}
void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
#if defined(TELEMETRY_FRSKY)
handleFrSkyTelemetry(rxConfig, deadband3d_throttle);
@ -148,7 +148,9 @@ void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
#endif
#if defined(TELEMETRY_HOTT)
handleHoTTTelemetry();
handleHoTTTelemetry(currentTime);
#else
UNUSED(currentTime);
#endif
#if defined(TELEMETRY_SMARTPORT)
@ -160,7 +162,9 @@ void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
#endif
#if defined(TELEMETRY_MAVLINK)
handleMAVLinkTelemetry();
handleMAVLinkTelemetry(currentTime);
#else
UNUSED(currentTime);
#endif
#if defined(TELEMETRY_JETIEXBUS)

2
src/main/telemetry/telemetry.h
View file

@ -56,7 +56,7 @@ extern struct serialPort_s *telemetrySharedPort;
void telemetryCheckState(void);
struct rxConfig_s;
void telemetryProcess(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
void telemetryProcess(uint32_t currentTime, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
bool telemetryDetermineEnabledState(portSharing_e portSharing);