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

Process the settings updates more discretely in revo attitude

Browse Source
This commit is contained in:
James Cotton 2012-07-21 13:50:16 -05:00
parent 6eb0fd2b3b
commit e5bd999975
1 changed files with 34 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
flight/Modules/Attitude/revolution/attitude.c
View File

@ -99,6 +99,7 @@ static AttitudeSettingsData attitudeSettings;
static HomeLocationData homeLocation; static HomeLocationData homeLocation;
static RevoCalibrationData revoCalibration; static RevoCalibrationData revoCalibration;
static RevoSettingsData revoSettings; static RevoSettingsData revoSettings;
static bool gyroBiasSettingsUpdated = false;
const uint32_t SENSOR_QUEUE_SIZE = 10; const uint32_t SENSOR_QUEUE_SIZE = 10;
// Private functions // Private functions
@ -204,6 +205,8 @@ static void AttitudeTask(void *parameters)
// Force settings update to make sure rotation loaded // Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle()); settingsUpdatedCb(AttitudeSettingsHandle());
settingsUpdatedCb(RevoSettingsHandle()); settingsUpdatedCb(RevoSettingsHandle());
settingsUpdatedCb(RevoCalibrationHandle());
settingsUpdatedCb(HomeLocationHandle());
// Wait for all the sensors be to read // Wait for all the sensors be to read
vTaskDelay(100); vTaskDelay(100);
@ -834,35 +837,43 @@ static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
return 0; return 0;
} }
static void settingsUpdatedCb(UAVObjEvent * objEv) static void settingsUpdatedCb(UAVObjEvent * ev)
{ {
float lat, alt; // If the object updated was the ObjectPersistence execute requested action
if (ev->obj == RevoCalibrationHandle()) {
RevoCalibrationGet(&revoCalibration);
AttitudeSettingsGet(&attitudeSettings); /* When the revo calibration is updated, update the GyroBias object */
RevoCalibrationGet(&revoCalibration); GyrosBiasData gyrosBias;
RevoSettingsGet(&revoSettings); GyrosBiasGet(&gyrosBias);
HomeLocationGet(&homeLocation); gyrosBias.x = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyrosBias.y = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyrosBias.z = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
GyrosBiasSet(&gyrosBias);
GyrosBiasData gyrosBias; gyroBiasSettingsUpdated = true;
GyrosBiasGet(&gyrosBias);
gyrosBias.x = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f;
gyrosBias.y = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
gyrosBias.z = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f;
GyrosBiasSet(&gyrosBias);
// Compute matrix to convert deltaLLA to NED // In case INS currently running
lat = homeLocation.Latitude / 10.0e6f * DEG2RAD; INSSetMagVar(revoCalibration.mag_var);
alt = homeLocation.Altitude; INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
} else if (ev->obj == HomeLocationHandle()) {
HomeLocationGet(&homeLocation);
// In case INS currently running // Compute matrix to convert deltaLLA to NED
INSSetMagVar(revoCalibration.mag_var); float lat, alt;
INSSetAccelVar(revoCalibration.accel_var); lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
INSSetGyroVar(revoCalibration.gyro_var); alt = homeLocation.Altitude;
INSSetBaroVar(revoCalibration.baro_var);
T[0] = alt+6.378137E6f; T[0] = alt+6.378137E6f;
T[1] = cosf(lat)*(alt+6.378137E6f); T[1] = cosf(lat)*(alt+6.378137E6f);
T[2] = -1.0f; T[2] = -1.0f;
} else if (ev->obj == AttitudeSettingsHandle()) {
AttitudeSettingsGet(&attitudeSettings);
} else if (ev->obj == RevoSettingsHandle()) {
RevoSettingsGet(&revoSettings);
}
} }
/** /**
* @} * @}