From 0178d666d5c2cbca4875a3c9786600d3ef40c63d Mon Sep 17 00:00:00 2001 From: "Richard Flay (Hyper)" Date: Fri, 3 May 2013 19:24:54 +0930 Subject: [PATCH] OP-931: Fixes the mag_updated test in revo Attitude module to use the square of the Be vector magnitude. Also fixes the dT calculation issues in a few places, one of which was as suggested in OPReview-459. +review OPReview-459 --- flight/modules/Actuator/actuator.c | 15 ++++++--------- flight/modules/Attitude/attitude.c | 2 +- flight/modules/Attitude/revolution/attitude.c | 2 +- flight/modules/ManualControl/manualcontrol.c | 6 ++---- 4 files changed, 10 insertions(+), 15 deletions(-) diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index 4ceba8bfb..39d2faabf 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -155,7 +155,8 @@ static void actuatorTask(void* parameters) UAVObjEvent ev; portTickType lastSysTime; portTickType thisSysTime; - float dT = 0.0f; + float dTSeconds; + uint32_t dTMilliseconds; ActuatorCommandData command; ActuatorDesiredData desired; @@ -206,13 +207,9 @@ static void actuatorTask(void* parameters) // Check how long since last update thisSysTime = xTaskGetTickCount(); - // reuse dt in case of wraparound - // todo: - // if dT actually matters... - // fix it to know max value and subtract for currently correct dT on wrap - if(thisSysTime > lastSysTime) - dT = (thisSysTime - lastSysTime) * (portTICK_RATE_MS * 0.001f); + dTMilliseconds = (thisSysTime == lastSysTime)? 1: (thisSysTime - lastSysTime) * portTICK_RATE_MS; lastSysTime = thisSysTime; + dTSeconds = dTMilliseconds * 0.001f; FlightStatusGet(&flightStatus); ActuatorDesiredGet(&desired); @@ -296,7 +293,7 @@ static void actuatorTask(void* parameters) } if((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) - status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT); + status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds); else status[ct] = -1; @@ -370,7 +367,7 @@ static void actuatorTask(void* parameters) actuatorSettings.ChannelNeutral[i]); // Store update time - command.UpdateTime = dT * 1000.0f; + command.UpdateTime = dTMilliseconds; if (command.UpdateTime > command.MaxUpdateTime) command.MaxUpdateTime = command.UpdateTime; diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index 7d4a01b6c..2340ef731 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -457,7 +457,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) portTickType thisSysTime = xTaskGetTickCount(); static portTickType lastSysTime = 0; - dT = (thisSysTime == lastSysTime) ? 0.001f : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f; + dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f; lastSysTime = thisSysTime; // Bad practice to assume structure order, but saves memory diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index ec8efa503..bba99405e 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -604,7 +604,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) mag_updated &= (!isnan(magData.x) && !isinf(magData.x) && !isnan(magData.y) && !isinf(magData.y) && !isnan(magData.z) && !isinf(magData.z)); // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily // switching between indoor and outdoor mode with Set = false) - mag_updated &= (homeLocation.Be[0] > 0.0f || homeLocation.Be[1] > 0.0f || homeLocation.Be[2] > 0.0f); + mag_updated &= (homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] > 1e-5f); // Discard airspeed if sensor not connected airspeed_updated &= ( airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE ); diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 912d67f73..c9c95c088 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -763,7 +763,7 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) StabilizationSettingsGet(&stabSettings); thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f); + dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); lastSysTime = thisSysTime; altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax; @@ -834,9 +834,7 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr } static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { - if(end_time > start_time) - return (end_time - start_time) * portTICK_RATE_MS; - return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS; + return (end_time - start_time) * portTICK_RATE_MS; } /**