From 1591d41b33916e9370e8a8db1cbb8139ec3d607b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Mar 2012 18:35:39 +1100 Subject: [PATCH] ACM: make it possible to build ArduCopter with quaternions --- ArduCopter/ArduCopter.pde | 39 ++++++++++++++++++++++++++++++++++++-- ArduCopter/GCS_Mavlink.pde | 2 ++ ArduCopter/Makefile | 3 +++ ArduCopter/config.h | 4 ++++ ArduCopter/system.pde | 2 ++ ArduCopter/test.pde | 17 +++++++++++++++-- 6 files changed, 63 insertions(+), 4 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 7578d4ca16..15a5e6682d 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -72,6 +72,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // Parent header of Timer // (only included for makefile libpath to work) #include // TimerProcess is the scheduler for MPU6000 reads. +#include // Madgwick quaternion system #include // ArduPilot Mega DCM Library #include // PI library #include // PID library @@ -222,10 +223,20 @@ AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN ); AP_InertialSensor_Oilpan ins(&adc); #endif AP_IMU_INS imu(&ins); + // we don't want to use gps for yaw correction on ArduCopter, so pass // a NULL GPS object pointer static GPS *g_gps_null; -AP_DCM dcm(&imu, g_gps_null); + +#if QUATERNION_ENABLE == ENABLED + // this shouldn't be called dcm of course, but until we + // decide to actually use something else, I don't want the patch + // size to be huge + AP_Quaternion dcm(&imu, g_gps_null); +#else + AP_DCM dcm(&imu, g_gps_null); +#endif + AP_TimerProcess timer_scheduler; #elif HIL_MODE == HIL_MODE_SENSORS @@ -842,6 +853,7 @@ void setup() { void loop() { uint32_t timer = micros(); + // We want this to execute fast // ---------------------------- if ((timer - fast_loopTimer) >= 4000 && imu.new_data_available()) { @@ -960,8 +972,13 @@ static void medium_loop() #if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode if(g.compass_enabled){ if (compass.read()) { - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + // Calculate heading +#if QUATERNION_ENABLE == ENABLED + compass.calculate(dcm.roll, dcm.pitch); +#else + compass.calculate(dcm.get_dcm_matrix()); compass.null_offsets(dcm.get_dcm_matrix()); +#endif } } #endif @@ -1844,6 +1861,7 @@ static void read_AHRS(void) } static void update_trig(void){ +#if QUATERNION_ENABLE == DISABLED Vector2f yawvector; Matrix3f temp = dcm.get_dcm_matrix(); @@ -1862,6 +1880,23 @@ static void update_trig(void){ sin_yaw_y = yawvector.x; // 1y = north cos_yaw_x = yawvector.y; // 0x = north +#else + // we need a cheaper way to calculate this .... + Vector2f yawvector; + float cp = cos(dcm.pitch); + float cr = cos(dcm.roll); + float sy = sin(dcm.yaw); + float cy = cos(dcm.yaw); + + yawvector.x = cp * cy; + yawvector.y = cp * sy; + yawvector.normalize(); + + cos_pitch_x = cp; + cos_roll_x = cr; + sin_yaw_y = yawvector.x; + cos_yaw_x = yawvector.y; +#endif //flat: // 0 ° = cos_yaw: 0.00, sin_yaw: 1.00, // 90° = cos_yaw: 1.00, sin_yaw: 0.00, diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 9ac2fd7e07..78fc8f2530 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -99,6 +99,7 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan) static void NOINLINE send_location(mavlink_channel_t chan) { +#if QUATERNION_ENABLE == DISABLED Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now mavlink_msg_global_position_int_send( chan, @@ -108,6 +109,7 @@ static void NOINLINE send_location(mavlink_channel_t chan) g_gps->ground_speed * rot.a.x, g_gps->ground_speed * rot.b.x, g_gps->ground_speed * rot.c.x); +#endif } static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) diff --git a/ArduCopter/Makefile b/ArduCopter/Makefile index 5d4f5f3732..39784e4912 100644 --- a/ArduCopter/Makefile +++ b/ArduCopter/Makefile @@ -42,6 +42,9 @@ sitl-hexa: sitl-y6: make -f ../libraries/Desktop/Makefile.desktop y6 +sitl-quaternion: + make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DQUATERNION_ENABLE=ENABLED" + etags: cd .. && etags -f ArduCopter/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduCopter libraries) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index fd5be47917..a7b3baf794 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -939,5 +939,9 @@ #endif +// experimental quaternion code +#ifndef QUATERNION_ENABLE +# define QUATERNION_ENABLE DISABLED +#endif #endif // __ARDUCOPTER_CONFIG_H__ diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index d3e9814d20..1391643a85 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -309,11 +309,13 @@ static void init_ardupilot() reset_control_switch(); #if HIL_MODE != HIL_MODE_ATTITUDE +#if QUATERNION_ENABLE == DISABLED dcm.kp_roll_pitch(0.130000); dcm.ki_roll_pitch(0.00001278), // 50 hz I term dcm.kp_yaw(0.12); dcm.ki_yaw(0.00002); dcm._clamp = 5; +#endif #endif // init the Z damopener diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 766b23205d..00a3dcb89d 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -330,8 +330,12 @@ test_radio(uint8_t argc, const Menu::arg *argv) medium_loopCounter++; if(medium_loopCounter == 5){ compass.read(); // Read magnetometer - compass.calculate(dcm.roll, dcm.pitch); // Calculate heading - compass.null_offsets(dcm.get_dcm_matrix()); +#if QUATERNION_ENABLE == ENABLED + compass.calculate(dcm.roll, dcm.pitch); +#else + compass.calculate(dcm.get_dcm_matrix()); + compass.null_offsets(dcm.get_dcm_matrix()); +#endif medium_loopCounter = 0; } } @@ -548,7 +552,12 @@ test_imu(uint8_t argc, const Menu::arg *argv) if(g.compass_enabled){ compass.read(); // Read magnetometer +#if QUATERNION_ENABLE == ENABLED + compass.calculate(dcm.roll, dcm.pitch); +#else compass.calculate(dcm.get_dcm_matrix()); + compass.null_offsets(dcm.get_dcm_matrix()); +#endif } } fast_loopTimer = millis(); @@ -932,7 +941,11 @@ test_mag(uint8_t argc, const Menu::arg *argv) while(1){ delay(100); if (compass.read()) { +#if QUATERNION_ENABLE == ENABLED + compass.calculate(dcm.roll, dcm.pitch); +#else compass.calculate(dcm.get_dcm_matrix()); +#endif Vector3f maggy = compass.get_offsets(); Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), (wrap_360(ToDeg(compass.heading) * 100)) /100,