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

Make the gyro go a bit more regularly to stabilization but needs updating

Browse Source 
regular CC
This commit is contained in:
James Cotton 2012-01-25 09:56:07 -06:00
parent e4df3202e0
commit 727f0befc5
2 changed files with 7 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
flight/Modules/Attitude/attitude.c
View File

@ -249,7 +249,6 @@ static void AttitudeTask(void *parameters)
// Only update attitude when sensor data is good
updateAttitude(&accels, &gyros);
AccelsSet(&accels);
GyrosSet(&gyros);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
}
@ -377,7 +376,7 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
struct pios_l3gd20_data gyro;
gyro_scaling = PIOS_L3GD20_GetScale();
if(xQueueReceive(gyro_queue, (void *) &gyro, 3) == errQUEUE_EMPTY) {
if(xQueueReceive(gyro_queue, (void *) &gyro, 2) == errQUEUE_EMPTY) {
// Unfortunately if the L3GD20 ever misses getting read, then it will not
// trigger more interrupts. In this case we must force a read to kickstart
// it.
@ -396,7 +395,7 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
gyros_accum[1] = gyrosData->y;
gyros_accum[2] = gyrosData->z;
if(xQueueReceive(gyro_queue, (void * const) &gyro, 3) == errQUEUE_EMPTY) {
if(xQueueReceive(gyro_queue, (void * const) &gyro, 2) == errQUEUE_EMPTY) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return -1;
}
@ -410,7 +409,7 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
gyros_accum[1] += gyrosData->y;
gyros_accum[2] += gyrosData->z;
if(xQueueReceive(gyro_queue, (void * const) &gyro, 3) == errQUEUE_EMPTY) {
if(xQueueReceive(gyro_queue, (void * const) &gyro, 2) == errQUEUE_EMPTY) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return -1;
}
@ -419,6 +418,7 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
gyrosData->y = -gyro.gyro_y * gyro_scaling;
gyrosData->z = -gyro.gyro_z * gyro_scaling;
gyrosData->temperature = 0;
GyrosSet(gyrosData);
gyros_accum[0] += gyrosData->x;
gyros_accum[1] += gyrosData->y;

11
flight/Modules/Stabilization/stabilization.c
View File

@ -140,10 +140,9 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
*/
static void stabilizationTask(void* parameters)
{
portTickType lastSysTime;
portTickType thisSysTime;
UAVObjEvent ev;
uint32_t timeval = PIOS_DELAY_GetRaw();
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
@ -155,7 +154,6 @@ static void stabilizationTask(void* parameters)
SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop
lastSysTime = xTaskGetTickCount();
ZeroPids();
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
@ -167,11 +165,8 @@ static void stabilizationTask(void* parameters)
continue;
}
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
timeval = PIOS_DELAY_GetRaw();
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);