rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-931: Fixes the mag_updated test in revo Attitude module to use the square of the Be vector magnitude.

Browse Source 
Also fixes the dT calculation issues in a few places, one of which was as suggested in OPReview-459.

+review OPReview-459
This commit is contained in:
Richard Flay (Hyper) 2013-05-03 19:24:54 +09:30
parent 742058da8c
commit 0178d666d5
4 changed files with 10 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
flight/modules/Actuator/actuator.c
View File

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

2
flight/modules/Attitude/attitude.c
View File

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

2
flight/modules/Attitude/revolution/attitude.c
View File

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

6
flight/modules/ManualControl/manualcontrol.c
View File

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