aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopterNG - changed to use AP_Compass (instead of APM_Compass)

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1114 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2010-12-12 14:05:31 +00:00
parent abacc627d8
commit ddd2b585f8
5 changed files with 36 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArducopterNG/ArducopterNG.pde
View File

@ -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;

32
ArducopterNG/CLI.pde
View File

@ -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

2
ArducopterNG/DCM.pde
View File

@ -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);

12
ArducopterNG/GCS.pde
View File

@ -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();

10
ArducopterNG/System.pde
View File

@ -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
@ -199,4 +199,4 @@ void APM_Init() {
}