ArduCopterNG - changed to use AP_Compass (instead of APM_Compass)
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1114 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com
parent
abacc627d8
commit
ddd2b585f8
@ -31,7 +31,7 @@
|
||||
/* AP_ADC : External ADC library */
|
||||
/* DataFlash : DataFlash log library */
|
||||
/* APM_BMP085 : BMP085 barometer library */
|
||||
/* APM_Compass : HMC5843 compass library [optional] */
|
||||
/* AP_Compass : HMC5843 compass library [optional] */
|
||||
/* GPS_MTK or GPS_UBLOX or GPS_NMEA : GPS library [optional] */
|
||||
/* ********************************************************************** */
|
||||
|
||||
@ -144,9 +144,9 @@
|
||||
// Magneto orientation and corrections.
|
||||
// If you don't have magneto activated, It is safe to ignore these
|
||||
//#ifdef IsMAG
|
||||
#define MAGORIENTATION APM_COMPASS_COMPONENTS_UP_PINS_FORWARD // This is default solution for ArduCopter
|
||||
//#define MAGORIENTATION APM_COMPASS_COMPONENTS_UP_PINS_BACK // Alternative orientation for ArduCopter
|
||||
//#define MAGORIENTATION APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD // If you have soldered Magneto to IMU shield in WIki pictures shows
|
||||
#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_FORWARD // This is default solution for ArduCopter
|
||||
//#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK // Alternative orientation for ArduCopter
|
||||
//#define MAGORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD // If you have soldered Magneto to IMU shield in WIki pictures shows
|
||||
|
||||
// To get Magneto offsets, switch to CLI mode and run offset calibration. During calibration
|
||||
// you need to roll/bank/tilt/yaw/shake etc your ArduCopter. Don't kick like Jani always does :)
|
||||
@ -197,7 +197,7 @@
|
||||
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
|
||||
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
|
||||
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
|
||||
#include <APM_Compass.h> // ArduPilot Mega Magnetometer Library
|
||||
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
|
||||
#include <Wire.h> // I2C Communication library
|
||||
#include <EEPROM.h> // EEPROM
|
||||
//#include <AP_GPS.h>
|
||||
@ -222,7 +222,7 @@
|
||||
// Sensors - declare one global instance
|
||||
AP_ADC_ADS7844 adc;
|
||||
APM_BMP085_Class APM_BMP085;
|
||||
APM_Compass_Class APM_Compass;
|
||||
AP_Compass_HMC5843 AP_Compass;
|
||||
|
||||
/* ************************************************************ */
|
||||
/* ************* MAIN PROGRAM - DECLARATIONS ****************** */
|
||||
@ -441,8 +441,8 @@ void loop()
|
||||
slowLoop++;
|
||||
#ifdef IsMAG
|
||||
if (MAGNETOMETER == 1) {
|
||||
APM_Compass.Read(); // Read magnetometer
|
||||
APM_Compass.Calculate(roll,pitch); // Calculate heading
|
||||
AP_Compass.read(); // Read magnetometer
|
||||
AP_Compass.calculate(roll,pitch); // Calculate heading
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
@ -138,10 +138,10 @@ void CALIB_CompassOffset() {
|
||||
delay(1000);
|
||||
SerPrln("starting now....");
|
||||
|
||||
APM_Compass.Init(); // Initialization
|
||||
APM_Compass.SetOrientation(MAGORIENTATION); // set compass's orientation on aircraft
|
||||
APM_Compass.SetOffsets(0,0,0); // set offsets to account for surrounding interference
|
||||
APM_Compass.SetDeclination(ToRad(DECLINATION)); // set local difference between magnetic north and true north
|
||||
AP_Compass.init(); // Initialization
|
||||
AP_Compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
|
||||
AP_Compass.set_offsets(0,0,0); // set offsets to account for surrounding interference
|
||||
AP_Compass.set_declination(ToRad(DECLINATION)); // set local difference between magnetic north and true north
|
||||
int counter = 0;
|
||||
|
||||
SerFlu();
|
||||
@ -149,18 +149,18 @@ void CALIB_CompassOffset() {
|
||||
static float min[3], max[3], offset[3];
|
||||
if((millis()- timer) > 100) {
|
||||
timer = millis();
|
||||
APM_Compass.Read();
|
||||
APM_Compass.Calculate(0,0); // roll = 0, pitch = 0 for this example
|
||||
AP_Compass.read();
|
||||
AP_Compass.calculate(0,0); // roll = 0, pitch = 0 for this example
|
||||
|
||||
// capture min
|
||||
if( APM_Compass.Mag_X < min[0] ) min[0] = APM_Compass.Mag_X;
|
||||
if( APM_Compass.Mag_Y < min[1] ) min[1] = APM_Compass.Mag_Y;
|
||||
if( APM_Compass.Mag_Z < min[2] ) min[2] = APM_Compass.Mag_Z;
|
||||
if( AP_Compass.mag_x < min[0] ) min[0] = AP_Compass.mag_x;
|
||||
if( AP_Compass.mag_y < min[1] ) min[1] = AP_Compass.mag_y;
|
||||
if( AP_Compass.mag_z < min[2] ) min[2] = AP_Compass.mag_z;
|
||||
|
||||
// capture max
|
||||
if( APM_Compass.Mag_X > max[0] ) max[0] = APM_Compass.Mag_X;
|
||||
if( APM_Compass.Mag_Y > max[1] ) max[1] = APM_Compass.Mag_Y;
|
||||
if( APM_Compass.Mag_Z > max[2] ) max[2] = APM_Compass.Mag_Z;
|
||||
if( AP_Compass.mag_x > max[0] ) max[0] = AP_Compass.mag_x;
|
||||
if( AP_Compass.mag_y > max[1] ) max[1] = AP_Compass.mag_y;
|
||||
if( AP_Compass.mag_z > max[2] ) max[2] = AP_Compass.mag_z;
|
||||
|
||||
// calculate offsets
|
||||
offset[0] = -(max[0]+min[0])/2;
|
||||
@ -169,13 +169,13 @@ void CALIB_CompassOffset() {
|
||||
|
||||
// display all to user
|
||||
SerPri("Heading:");
|
||||
SerPri(ToDeg(APM_Compass.Heading));
|
||||
SerPri(ToDeg(AP_Compass.heading));
|
||||
SerPri(" \t(");
|
||||
SerPri(APM_Compass.Mag_X);
|
||||
SerPri(AP_Compass.mag_x);
|
||||
SerPri(",");
|
||||
SerPri(APM_Compass.Mag_Y);
|
||||
SerPri(AP_Compass.mag_y);
|
||||
SerPri(",");
|
||||
SerPri(APM_Compass.Mag_Z);
|
||||
SerPri(AP_Compass.mag_z);
|
||||
SerPri(")");
|
||||
|
||||
// display offsets
|
||||
|
||||
@ -87,7 +87,7 @@ void Drift_correction(void)
|
||||
//*****YAW***************
|
||||
// We make the gyro YAW drift correction based on compass magnetic heading
|
||||
if (MAGNETOMETER == 1) {
|
||||
errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X); //Calculating YAW error
|
||||
errorCourse= (DCM_Matrix[0][0]*AP_Compass.heading_y) - (DCM_Matrix[1][0]*AP_Compass.heading_x); //Calculating YAW error
|
||||
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
||||
|
||||
@ -366,17 +366,17 @@ void sendSerialTelemetry() {
|
||||
SerPri(read_adc(5));
|
||||
|
||||
comma();
|
||||
SerPri(APM_Compass.Heading, 4);
|
||||
SerPri(AP_Compass.heading, 4);
|
||||
comma();
|
||||
SerPri(APM_Compass.Heading_X, 4);
|
||||
SerPri(AP_Compass.heading_x, 4);
|
||||
comma();
|
||||
SerPri(APM_Compass.Heading_Y, 4);
|
||||
SerPri(AP_Compass.heading_y, 4);
|
||||
comma();
|
||||
SerPri(APM_Compass.Mag_X);
|
||||
SerPri(AP_Compass.mag_x);
|
||||
comma();
|
||||
SerPri(APM_Compass.Mag_Y);
|
||||
SerPri(AP_Compass.mag_y);
|
||||
comma();
|
||||
SerPri(APM_Compass.Mag_Z);
|
||||
SerPri(AP_Compass.mag_z);
|
||||
comma();
|
||||
|
||||
SerPriln();
|
||||
|
||||
@ -140,10 +140,10 @@ void APM_Init() {
|
||||
|
||||
#ifdef IsMAG
|
||||
if (MAGNETOMETER == 1) {
|
||||
APM_Compass.Init(FALSE); // I2C initialization
|
||||
APM_Compass.SetOrientation(MAGORIENTATION);
|
||||
APM_Compass.SetOffsets(mag_offset_x, mag_offset_y, mag_offset_z);
|
||||
APM_Compass.SetDeclination(ToRad(DECLINATION));
|
||||
AP_Compass.init(FALSE); // I2C initialization
|
||||
AP_Compass.set_orientation(MAGORIENTATION);
|
||||
AP_Compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
|
||||
AP_Compass.set_declination(ToRad(DECLINATION));
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user