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

ACM: ArduCopter updates for new compass interface

Browse Source
This commit is contained in:
Andrew Tridgell 2012-06-20 19:31:37 +10:00
parent 44b7d94b1c
commit 7883c4a545
5 changed files with 6 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/ArduCopter.pde
View File

@ -1040,9 +1040,6 @@ static void medium_loop()
#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode #if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
if(g.compass_enabled){ if(g.compass_enabled){
if (compass.read()) { if (compass.read()) {
// Calculate heading
Matrix3f m = ahrs.get_dcm_matrix();
compass.calculate(m);
compass.null_offsets(); compass.null_offsets();
} }
} }

2
ArduCopter/Log.pde
View File

@ -744,7 +744,7 @@ static void Log_Write_Attitude()
DataFlash.WriteInt((int)ahrs.pitch_sensor); // 4 DataFlash.WriteInt((int)ahrs.pitch_sensor); // 4
DataFlash.WriteInt(g.rc_4.control_in); // 5 DataFlash.WriteInt(g.rc_4.control_in); // 5
DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor); // 6 DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor); // 6
DataFlash.WriteInt((uint16_t)(wrap_360(ToDeg(compass.heading)*100))); // 7 DataFlash.WriteInt(0); // 7 (this used to be compass.heading)
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }

1
ArduCopter/sensors.pde
View File

@ -42,7 +42,6 @@ static void init_compass()
return; return;
} }
ahrs.set_compass(&compass); ahrs.set_compass(&compass);
compass.null_offsets_enable();
} }
static void init_optflow() static void init_optflow()

6
ArduCopter/setup.pde
View File

@ -719,13 +719,13 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
int _max[3] = {0,0,0}; int _max[3] = {0,0,0};
compass.read(); compass.read();
compass.calculate(0,0); // roll = 0, pitch = 0
while(1){ while(1){
delay(50); delay(50);
float heading;
compass.read(); compass.read();
compass.calculate(0,0); // roll = 0, pitch = 0 heading = compass.calculate_heading(0,0); // roll = 0, pitch = 0
if(compass.mag_x < _min[0]) _min[0] = compass.mag_x; if(compass.mag_x < _min[0]) _min[0] = compass.mag_x;
if(compass.mag_y < _min[1]) _min[1] = compass.mag_y; if(compass.mag_y < _min[1]) _min[1] = compass.mag_y;
@ -744,7 +744,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
// display all to user // display all to user
Serial.printf_P(PSTR("Heading: %u, \t (%d, %d, %d), (%4.4f, %4.4f, %4.4f)\n"), Serial.printf_P(PSTR("Heading: %u, \t (%d, %d, %d), (%4.4f, %4.4f, %4.4f)\n"),
(uint16_t)(wrap_360(ToDeg(compass.heading) * 100)) /100, (uint16_t)(wrap_360(ToDeg(heading) * 100)) /100,
compass.mag_x, compass.mag_x,
compass.mag_y, compass.mag_y,

7
ArduCopter/test.pde
View File

@ -334,7 +334,6 @@ test_radio(uint8_t argc, const Menu::arg *argv)
if(medium_loopCounter == 5){ if(medium_loopCounter == 5){
Matrix3f m = dcm.get_dcm_matrix(); Matrix3f m = dcm.get_dcm_matrix();
compass.read(); // Read magnetometer compass.read(); // Read magnetometer
compass.calculate(m);
compass.null_offsets(); compass.null_offsets();
medium_loopCounter = 0; medium_loopCounter = 0;
} }
@ -552,8 +551,6 @@ test_imu(uint8_t argc, const Menu::arg *argv)
if(g.compass_enabled){ if(g.compass_enabled){
compass.read(); // Read magnetometer compass.read(); // Read magnetometer
Matrix3f m = dcm.get_dcm_matrix();
compass.calculate(m);
compass.null_offsets(); compass.null_offsets();
} }
} }
@ -954,10 +951,10 @@ test_mag(uint8_t argc, const Menu::arg *argv)
while(1){ while(1){
delay(100); delay(100);
if (compass.read()) { if (compass.read()) {
compass.calculate(ahrs.get_dcm_matrix()); float heading = compass.calculate_heading(ahrs.get_dcm_matrix());
Vector3f maggy = compass.get_offsets(); Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100, (wrap_360(ToDeg(heading) * 100)) /100,
compass.mag_x, compass.mag_x,
compass.mag_y, compass.mag_y,
compass.mag_z); compass.mag_z);