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

Some bug fixes for revo attitude estimation.

Browse Source
This commit is contained in:
James Cotton 2012-03-14 18:25:36 -05:00
parent d4488512c0
commit 7f226867c6
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -121,6 +121,7 @@ int32_t AttitudeInitialize(void)
PositionActualInitialize(); PositionActualInitialize();
VelocityActualInitialize(); VelocityActualInitialize();
RevoSettingsInitialize(); RevoSettingsInitialize();
RevoCalibrationInitialize();
// Initialize this here while we aren't setting the homelocation in GPS // Initialize this here while we aren't setting the homelocation in GPS
HomeLocationInitialize(); HomeLocationInitialize();
@ -144,6 +145,7 @@ int32_t AttitudeInitialize(void)
AttitudeSettingsConnectCallback(&settingsUpdatedCb); AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb); RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb); HomeLocationConnectCallback(&settingsUpdatedCb);
return 0; return 0;
@ -256,6 +258,8 @@ static int32_t updateAttitudeComplimentary(bool first_run)
return -1; return -1;
} }
AccelsGet(&accelsData);
// During initialization and // During initialization and
FlightStatusData flightStatus; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
@ -269,7 +273,7 @@ static int32_t updateAttitudeComplimentary(bool first_run)
float q[4]; float q[4];
rpy[0] = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; rpy[0] = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
rpy[1] = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; rpy[1] = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
rpy[2] = atan2f(magData.x, -magData.y) * 180.0f / F_PI; rpy[2] = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(rpy,q); RPY2Quaternion(rpy,q);
quat_copy(q, &attitudeActual.q1); quat_copy(q, &attitudeActual.q1);
@ -297,7 +301,6 @@ static int32_t updateAttitudeComplimentary(bool first_run)
} }
GyrosGet(&gyrosData); GyrosGet(&gyrosData);
AccelsGet(&accelsData);
// Compute the dT using the cpu clock // Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
@ -541,7 +544,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
float rpy[3]; float rpy[3];
rpy[0] = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; rpy[0] = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
rpy[1] = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; rpy[1] = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
rpy[2] = atan2f(magData.x, -magData.y) * 180.0f / F_PI; rpy[2] = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(rpy,q); RPY2Quaternion(rpy,q);
/*float Rbe[3][3]; /*float Rbe[3][3];
float ge[3] = {0,0,-9.81f}; float ge[3] = {0,0,-9.81f};
@ -573,7 +576,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// Set initial attitude // Set initial attitude
rpy[0] = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; rpy[0] = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
rpy[1] = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; rpy[1] = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
rpy[2] = atan2f(magData.x, -magData.y) * 180.0f / F_PI; rpy[2] = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(rpy,q); RPY2Quaternion(rpy,q);
INSSetState(NED, zeros, q, zeros, zeros); INSSetState(NED, zeros, q, zeros, zeros);
@ -648,7 +651,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if(gps_updated && outdoor_mode) if(gps_updated && outdoor_mode)
{ {
INSSetPosVelVar(1e-2f, 1e-2f); INSSetPosVelVar(1e-2f, 1e-2f);
sensors = POS_SENSORS | HORIZ_SENSORS; sensors |= POS_SENSORS | HORIZ_SENSORS;
GPSPositionData gpsPosition; GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition); GPSPositionGet(&gpsPosition);