rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-01 18:29:16 +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 // Only update attitude when sensor data is good
updateAttitude(&accels, &gyros); updateAttitude(&accels, &gyros);
AccelsSet(&accels); AccelsSet(&accels);
GyrosSet(&gyros);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
} }
} }
@ -377,7 +376,7 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
struct pios_l3gd20_data gyro; struct pios_l3gd20_data gyro;
gyro_scaling = PIOS_L3GD20_GetScale(); 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 // 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 // trigger more interrupts. In this case we must force a read to kickstart
// it. // it.
@ -396,7 +395,7 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
gyros_accum[1] = gyrosData->y; gyros_accum[1] = gyrosData->y;
gyros_accum[2] = gyrosData->z; 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); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return -1; return -1;
} }
@ -410,7 +409,7 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
gyros_accum[1] += gyrosData->y; gyros_accum[1] += gyrosData->y;
gyros_accum[2] += gyrosData->z; 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); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return -1; return -1;
} }
@ -419,6 +418,7 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
gyrosData->y = -gyro.gyro_y * gyro_scaling; gyrosData->y = -gyro.gyro_y * gyro_scaling;
gyrosData->z = -gyro.gyro_z * gyro_scaling; gyrosData->z = -gyro.gyro_z * gyro_scaling;
gyrosData->temperature = 0; gyrosData->temperature = 0;
GyrosSet(gyrosData);
gyros_accum[0] += gyrosData->x; gyros_accum[0] += gyrosData->x;
gyros_accum[1] += gyrosData->y; 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) static void stabilizationTask(void* parameters)
{ {
portTickType lastSysTime;
portTickType thisSysTime;
UAVObjEvent ev; UAVObjEvent ev;
uint32_t timeval = PIOS_DELAY_GetRaw();
ActuatorDesiredData actuatorDesired; ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
@ -155,7 +154,6 @@ static void stabilizationTask(void* parameters)
SettingsUpdatedCb((UAVObjEvent *) NULL); SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount();
ZeroPids(); ZeroPids();
while(1) { while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
@ -167,11 +165,8 @@ static void stabilizationTask(void* parameters)
continue; continue;
} }
// Check how long since last update dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
thisSysTime = xTaskGetTickCount(); timeval = PIOS_DELAY_GetRaw();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);