Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AHRS: added GPS support to AHRS test

Browse Source
This commit is contained in:
Andrew Tridgell 2012-03-23 16:27:37 +11:00
parent 19954f30e5
commit 5316a45c20
1 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde
View File

@ -24,9 +24,12 @@
#include <Filter.h>
// uncomment this for a APM2 board
#define APM2_HARDWARE
//#define APM2_HARDWARE
FastSerialPort(Serial, 0);
#define WITH_GPS 0
FastSerialPort0(Serial);
FastSerialPort1(Serial1);
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess scheduler;
@ -46,11 +49,13 @@ AP_Compass_HMC5843 compass;
static GPS *g_gps;
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
AP_IMU_INS imu(&ins);
// choose which AHRS system to use
//AP_AHRS_DCM ahrs(&imu, g_gps);
AP_AHRS_Quaternion ahrs(&imu, g_gps);
AP_AHRS_DCM ahrs(&imu, g_gps);
//AP_AHRS_Quaternion ahrs(&imu, g_gps);
AP_Baro_BMP085_HIL barometer;
@ -107,7 +112,13 @@ void setup(void)
Serial.printf("Enabling compass\n");
compass.null_offsets_enable();
ahrs.set_compass(&compass);
} else {
Serial.printf("No compass detected\n");
}
g_gps = &g_gps_driver;
#if WITH_GPS
g_gps->init();
#endif
}
void loop(void)
@ -127,6 +138,9 @@ void loop(void)
compass.calculate(ahrs.get_dcm_matrix());
// read compass at 10Hz
last_compass = now;
#if WITH_GPS
g_gps->update();
#endif
}
ahrs.update();