rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

Simple attitude calculation based on gravity and magnetic vector

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1298 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-15 06:03:44 +00:00 committed by peabody124
parent 3f408ad655
commit ddbd4f8c80
2 changed files with 19 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
flight/AHRS/ahrs.c
View File

@ -230,13 +230,9 @@ int main()
// Main loop
while (1) {
uint8_t loop_ctr;
// Alive signal
if (loop_ctr++ > 100) {
PIOS_LED_Toggle(LED1);
loop_ctr = 0;
}
// Get 3 ID bytes
strcpy ((char *)mag_data.id, "ZZZ");
@ -266,23 +262,27 @@ int main()
ahrs_state = AHRS_IDLE;
/* Simulate a rotating airframe */
attitude_data.quaternion.q1 += .001;
attitude_data.quaternion.q2 += .002;
attitude_data.quaternion.q3 += .003;
attitude_data.quaternion.q4 += 1;
/* Very simple computation of the heading and attitude from accel. */
attitude_data.euler.yaw = atan2((mag_data.raw.axis[0]), (-1 * mag_data.raw.axis[1])) * 180 / M_PI;
attitude_data.euler.pitch = atan2(accel_data.filtered.y, accel_data.filtered.z) * 180 / M_PI;
attitude_data.euler.roll = -atan2(accel_data.filtered.x,accel_data.filtered.z) * 180 / M_PI;
if (attitude_data.euler.yaw < 0) attitude_data.euler.yaw += 360.0;
attitude_data.euler.roll += .004;
if (attitude_data.euler.roll > 360.0) attitude_data.euler.roll -= 360.0;
attitude_data.euler.pitch += .005;
if (attitude_data.euler.pitch > 360.0) attitude_data.euler.pitch -= 360.0;
attitude_data.euler.yaw += .006;
if (attitude_data.euler.yaw > 360.0) attitude_data.euler.yaw -= 360.0;
float c1 = cos(attitude_data.euler.yaw/2);
float s1 = sin(attitude_data.euler.yaw/2);
float c2 = cos(attitude_data.euler.pitch/2);
float s2 = sin(attitude_data.euler.pitch/2);
float c3 = cos(attitude_data.euler.roll/2);
float s3 = sin(attitude_data.euler.roll/2);
float c1c2 = c1*c2;
float s1s2 = s1*s2;
attitude_data.quaternion.q1 = c1c2*c3 - s1s2*s3;
attitude_data.quaternion.q2 = c1c2*s3 + s1s2*c3;
attitude_data.quaternion.q3 = s1*c2*c3 + c1*s2*s3;
attitude_data.quaternion.q4 =c1*s2*c3 - s1*c2*s3;
process_spi_request();
// Delay until next reading
//PIOS_DELAY_WaitmS(50);
}
return 0;

4
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -61,10 +61,8 @@
65B7E6AE120DF1E2000C1123 /* ahrs.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = ahrs.c; path = ../../AHRS/ahrs.c; sourceTree = SOURCE_ROOT; };
65B7E6B0120DF1E2000C1123 /* ahrs.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs.h; sourceTree = "<group>"; };
65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_fsm.h; sourceTree = "<group>"; };
65B7E6B2120DF1E2000C1123 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
65B7E6B3120DF1E2000C1123 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = "<group>"; };
65B7E6B4120DF1E2000C1123 /* pios_config.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_config.h; sourceTree = "<group>"; };
65B7E6B5120DF1E2000C1123 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; };
65B7E6B6120DF1E2000C1123 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../AHRS/Makefile; sourceTree = SOURCE_ROOT; };
65B7E6B7120DF1E2000C1123 /* pios_board.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_board.c; path = ../../AHRS/pios_board.c; sourceTree = SOURCE_ROOT; };
65E8EF1F11EEA61E00BBF654 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../OpenPilot/Makefile; sourceTree = SOURCE_ROOT; };
@ -523,7 +521,6 @@
65B7E6AD120DF1E2000C1123 /* ahrs_fsm.c */,
65B7E6AE120DF1E2000C1123 /* ahrs.c */,
65B7E6AF120DF1E2000C1123 /* inc */,
65B7E6B5120DF1E2000C1123 /* insgps.c */,
65B7E6B6120DF1E2000C1123 /* Makefile */,
65B7E6B7120DF1E2000C1123 /* pios_board.c */,
);
@ -535,7 +532,6 @@
children = (
65B7E6B0120DF1E2000C1123 /* ahrs.h */,
65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */,
65B7E6B2120DF1E2000C1123 /* insgps.h */,
65B7E6B3120DF1E2000C1123 /* pios_board.h */,
65B7E6B4120DF1E2000C1123 /* pios_config.h */,
);