From 4653ea76299d80e09748c04c18577affad53227e Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 18 Oct 2011 22:00:24 -0400 Subject: [PATCH] Incorporated dcm changes from arducopter into apo. --- libraries/APO/AP_HardwareAbstractionLayer.h | 13 +++++++++++++ libraries/APO/AP_Navigator.cpp | 18 +++++++++++++++--- 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h index 422bb55d36..a1562efd0a 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -80,6 +80,19 @@ public: pinMode(slideSwitchPin, INPUT); pinMode(pushButtonPin, INPUT); DDRL |= B00000100; // set port L, pint 2 to output for the relay + } else if (board == BOARD_ARDUPILOTMEGA_2) { + slideSwitchPin = 40; + pushButtonPin = 41; + aLedPin = 37; + bLedPin = 36; + cLedPin = 35; + eepromMaxAddr = 2048; + pinMode(aLedPin, OUTPUT); // extra led + pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; + pinMode(cLedPin, OUTPUT); // gps led + pinMode(slideSwitchPin, INPUT); + pinMode(pushButtonPin, INPUT); + DDRL |= B00000100; // set port L, pint 2 to output for the relay } } diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index 61883cec41..4260ef1461 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -69,13 +69,25 @@ DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal) : } if (_hal->getMode() == MODE_LIVE) { - if (_hal->adc) + + if (_hal->adc) { _hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib); - if (_hal->imu) + } + + if (_hal->imu) { _dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass); + + // tune down dcm + _dcm->kp_roll_pitch(0.030000); + _dcm->ki_roll_pitch(0.00001278), // 50 hz I term + + // tune down compass in dcm + _dcm->kp_yaw(0.08); + _dcm->ki_yaw(0); + } + if (_hal->compass) { _dcm->set_compass(_hal->compass); - } } }