APM: make it possible to build ArduPlane with quaternion support

This commit is contained in:
Andrew Tridgell 2012-03-03 18:33:40 +11:00
parent a85ba80246
commit aa4beb9753
5 changed files with 39 additions and 5 deletions

View File

@ -41,6 +41,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <AP_Quaternion.h> // Madgwick quaternion system
#include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
@ -186,7 +187,15 @@ AP_GPS_None g_gps_driver(NULL);
AP_InertialSensor_Oilpan ins( &adc );
#endif // CONFIG_IMU_TYPE
AP_IMU_INS imu( &ins );
AP_DCM dcm(&imu, g_gps);
#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);
#else
AP_DCM dcm(&imu, g_gps);
#endif
#elif HIL_MODE == HIL_MODE_SENSORS
// sensor emulators
@ -771,8 +780,13 @@ static void medium_loop()
#if HIL_MODE != HIL_MODE_ATTITUDE
if (g.compass_enabled && compass.read()) {
dcm.set_compass(&compass);
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
} else {
dcm.set_compass(NULL);
}

View File

@ -306,6 +306,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,
@ -318,6 +319,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
g_gps->ground_speed * rot.b.x, // Y speed cm/s
g_gps->ground_speed * rot.c.x,
g_gps->ground_course); // course in 1/100 degree
#endif
}
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)

View File

@ -30,6 +30,9 @@ mavlink10:
sitl:
make -f ../libraries/Desktop/Makefile.desktop
sitl-quaternion:
make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DQUATERNION_ENABLE=ENABLED"
etags:
cd .. && etags -f ArduPlane/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduPlane libraries)

View File

@ -833,3 +833,8 @@
#ifndef RESET_SWITCH_CHAN_PWM
# define RESET_SWITCH_CHAN_PWM 1750
#endif
// experimental quaternion code
#ifndef QUATERNION_ENABLE
# define QUATERNION_ENABLE DISABLED
#endif

View File

@ -534,7 +534,12 @@ test_imu(uint8_t argc, const Menu::arg *argv)
medium_loopCounter++;
if(medium_loopCounter == 5){
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());
#endif
}
medium_loopCounter = 0;
}
@ -599,8 +604,13 @@ test_mag(uint8_t argc, const Menu::arg *argv)
medium_loopCounter++;
if(medium_loopCounter == 5){
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
}
medium_loopCounter = 0;
}