From 50032ef703d51b5fbe78e91c4911f8400c0e53d2 Mon Sep 17 00:00:00 2001 From: Wenyao Xie Date: Mon, 5 Dec 2011 20:57:19 -0500 Subject: [PATCH 1/5] Working on baromemter altitude --- apo/QuadArducopter.h | 6 +-- libraries/APO/AP_Navigator.cpp | 70 +++++++++++++++++++++++++++++++--- libraries/APO/AP_Navigator.h | 17 +++++++++ 3 files changed, 84 insertions(+), 9 deletions(-) diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index c1edfdd168..915483b55d 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -34,14 +34,14 @@ static const uint32_t gpsBaud = 38400; static const uint32_t hilBaud = 115200; // optional sensors -static const bool gpsEnabled = false; -static const bool baroEnabled = false; +static const bool gpsEnabled = true; +static const bool baroEnabled = true; static const bool compassEnabled = true; static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; // compass orientation: See AP_Compass_HMC5843.h for possible values // battery monitoring -static const bool batteryMonitorEnabled = false; +static const bool batteryMonitorEnabled = true; static const uint8_t batteryPin = 0; static const float batteryVoltageDivRatio = 6; static const float batteryMinVolt = 10.0; diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index 2623ddf8d9..63080bf4d3 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -5,8 +5,9 @@ * Author: jgoppert */ -#include "AP_Navigator.h" #include "../FastSerial/FastSerial.h" +#include "AP_Navigator.h" +#include "AP_CommLink.h" #include "AP_HardwareAbstractionLayer.h" #include "../AP_DCM/AP_DCM.h" #include "../AP_Math/AP_Math.h" @@ -98,7 +99,58 @@ void DcmNavigator::calibrate() { if (_hal->imu) { _hal->imu->init(IMU::COLD_START,delay,_hal->scheduler); } + + if (_hal->baro) { + // XXX should be moved to hal ctor + _hal->baro->Init(1,false); + // for apm 2 _hal->baro->Init(1,true); + int flashcount = 0; + + while(getGroundPressure() == 0){ + _hal->baro->Read(); // Get initial data from absolute pressure sensor + setGroundPressure(_hal->baro->Press); + setGroundTemperature(_hal->baro->Temp); + //mavlink_delay(20); + delay(20); + //Serial.printf("_hal->baro->Press %ld\n", _hal->baro->Press); + } + + for(int i = 0; i < 30; i++){ // We take some readings... + + //#if HIL_MODE == HIL_MODE_SENSORS + //gcs_update(); // look for inbound hil packets + //#endif + + _hal->baro->Read(); // Get initial data from absolute pressure sensor + setGroundPressure((getGroundPressure() * 9l + _hal->baro->Press) / 10l); + setGroundTemperature((getGroundTemperature() * 9 + _hal->baro->Temp) / 10); + + //mavlink_delay(20); + delay(20); + if(flashcount == 5) { + digitalWrite(_hal->cLedPin, LOW); + digitalWrite(_hal->aLedPin, HIGH); + digitalWrite(_hal->bLedPin, LOW); + } + + if(flashcount >= 10) { + flashcount = 0; + digitalWrite(_hal->cLedPin, LOW); + digitalWrite(_hal->aLedPin, HIGH); + digitalWrite(_hal->bLedPin, LOW); + } + flashcount++; + } + + // TODO implement + //saveGroundPressure(); + //saveGroundTemperature(); + // + _hal->debug->printf_P(PSTR("abs_pressure %ld\n"),getGroundPressure()); + _hal->gcs->sendText(SEVERITY_LOW, PSTR("barometer calibration complete\n")); + } } + void DcmNavigator::updateFast(float dt) { if (_hal->getMode() != MODE_LIVE) @@ -123,11 +175,17 @@ void DcmNavigator::updateFast(float dt) { setAlt(_rangeFinderDown->distance); else { - float tmp = (_hal->baro->Press / 101325.0); - tmp = pow(tmp, 0.190295); - //setAlt(44330 * (1.0 - tmp)); //sets the altitude in meters XXX wrong, baro reads 0 press - setAlt(0.0); - } + float x, scaling, temp; + + _hal->baro->Read(); // Get new data from absolute pressure sensor + + //abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering + float abs_pressure = (getGroundPressure() * .7) + (_hal->baro->Press * .3); // large filtering + scaling = getGroundPressure() / (float)abs_pressure; + temp = (getGroundTemperature()) + 273.15f; + x = log(scaling) * temp * 29271.267f; + setAlt(x / 10); + } } // dcm class for attitude diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index a208e70533..b450ee9052 100644 --- a/libraries/APO/AP_Navigator.h +++ b/libraries/APO/AP_Navigator.h @@ -264,6 +264,21 @@ public: _windSpeed = windSpeed; } + float getGroundPressure() const { + return _groundPressure; + } + + void setGroundPressure(long groundPressure) { + _groundPressure = groundPressure; + } + + float getGroundTemperature() const { + return _groundTemperature; + } + + void setGroundTemperature(long groundTemperature) { + _groundTemperature = groundTemperature; + } protected: AP_HardwareAbstractionLayer * _hal; @@ -287,6 +302,8 @@ private: int32_t _lat_degInt; // deg / 1e7 int32_t _lon_degInt; // deg / 1e7 int32_t _alt_intM; // meters / 1e3 + float _groundTemperature; // XXX units? + float _groundPressure; // XXX units? }; class DcmNavigator: public AP_Navigator { From 410c18ac05ffb0834891f799517970f1056d7b91 Mon Sep 17 00:00:00 2001 From: Wenyao Xie Date: Tue, 6 Dec 2011 13:26:07 -0500 Subject: [PATCH 2/5] Fixed Barometer Reading --- apo/QuadArducopter.h | 2 +- libraries/APO/AP_Navigator.cpp | 18 ++++++++++++------ 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 915483b55d..268ca7b33a 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -34,7 +34,7 @@ static const uint32_t gpsBaud = 38400; static const uint32_t hilBaud = 115200; // optional sensors -static const bool gpsEnabled = true; +static const bool gpsEnabled = false; static const bool baroEnabled = true; static const bool compassEnabled = true; static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index 63080bf4d3..b197c2d25a 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -179,12 +179,12 @@ void DcmNavigator::updateFast(float dt) { _hal->baro->Read(); // Get new data from absolute pressure sensor - //abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering - float abs_pressure = (getGroundPressure() * .7) + (_hal->baro->Press * .3); // large filtering - scaling = getGroundPressure() / (float)abs_pressure; - temp = (getGroundTemperature()) + 273.15f; + float abs_pressure = getGroundPressure() * .7 + _hal->baro->Press * .3; // large filtering + scaling = getGroundPressure()/abs_pressure; + temp = getGroundTemperature() + 273.15f; x = log(scaling) * temp * 29271.267f; - setAlt(x / 10); + _hal->debug->printf_P(PSTR("Ground Pressure %f\tAbsolute Pressure = %f\tGround Temperature = %f\tscaling= %f\n"),getGroundPressure(),abs_pressure,temp,log(scaling)); + setAlt_intM(x / 10); //changed this from 10 to 100 } } @@ -221,7 +221,13 @@ void DcmNavigator::updateSlow(float dt) { if (_hal->gps->fix && _hal->gps->new_data) { setLat_degInt(_hal->gps->latitude); setLon_degInt(_hal->gps->longitude); - setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm + //Preferential Barometer Altitude + if (_hal->baro) { + setAlt_intM(getAlt_intM()); + } + else { + setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm + } setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s } } From 7593aae452cc97d354beed242c771b7a09c86afe Mon Sep 17 00:00:00 2001 From: Wenyao Xie Date: Tue, 6 Dec 2011 13:28:39 -0500 Subject: [PATCH 3/5] Fixed Barometer Altitude --- libraries/APO/AP_Navigator.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index b197c2d25a..439bcd3f2e 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -183,8 +183,9 @@ void DcmNavigator::updateFast(float dt) { scaling = getGroundPressure()/abs_pressure; temp = getGroundTemperature() + 273.15f; x = log(scaling) * temp * 29271.267f; - _hal->debug->printf_P(PSTR("Ground Pressure %f\tAbsolute Pressure = %f\tGround Temperature = %f\tscaling= %f\n"),getGroundPressure(),abs_pressure,temp,log(scaling)); - setAlt_intM(x / 10); //changed this from 10 to 100 + //Barometer Debug + //_hal->debug->printf_P(PSTR("Ground Pressure %f\tAbsolute Pressure = %f\tGround Temperature = %f\tscaling= %f\n"),getGroundPressure(),abs_pressure,temp,log(scaling)); + setAlt_intM(x / 10); } } From 2432ae130d8ff59e1513d86629ba5a345d58be25 Mon Sep 17 00:00:00 2001 From: Wenyao Xie Date: Tue, 6 Dec 2011 18:56:16 -0500 Subject: [PATCH 4/5] Fixed baro. --- ArduBoat/BoatGeneric.h | 1 - apo/QuadArducopter.h | 2 +- libraries/APO/APO.h | 2 + libraries/APO/APO_DefaultSetup.h | 32 +-- libraries/APO/AP_Autopilot.cpp | 12 +- libraries/APO/AP_CommLink.cpp | 22 +- libraries/APO/AP_HardwareAbstractionLayer.cpp | 90 +++++++ libraries/APO/AP_HardwareAbstractionLayer.h | 51 +--- libraries/APO/AP_Navigator.cpp | 226 ------------------ libraries/APO/AP_Navigator.h | 54 +---- libraries/APO/AP_Var_keys.h | 1 + libraries/APO/DcmNavigator.cpp | 217 +++++++++++++++++ libraries/APO/DcmNavigator.h | 51 ++++ libraries/APO/template.h | 2 +- libraries/AP_DCM/AP_DCM.cpp | 7 +- 15 files changed, 408 insertions(+), 362 deletions(-) create mode 100644 libraries/APO/DcmNavigator.cpp create mode 100644 libraries/APO/DcmNavigator.h diff --git a/ArduBoat/BoatGeneric.h b/ArduBoat/BoatGeneric.h index 740b19c442..ac11ad7a08 100644 --- a/ArduBoat/BoatGeneric.h +++ b/ArduBoat/BoatGeneric.h @@ -22,7 +22,6 @@ static const uint8_t heartBeatTimeout = 3; #define COMMLINK_CLASS MavlinkComm // hardware selection -#define ADC_CLASS AP_ADC_ADS7844 #define COMPASS_CLASS AP_Compass_HMC5843 #define BARO_CLASS APM_BMP085_Class #define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 268ca7b33a..915483b55d 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -34,7 +34,7 @@ static const uint32_t gpsBaud = 38400; static const uint32_t hilBaud = 115200; // optional sensors -static const bool gpsEnabled = false; +static const bool gpsEnabled = true; static const bool baroEnabled = true; static const bool compassEnabled = true; static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h index 3fe10bb203..0d4bc6c181 100644 --- a/libraries/APO/APO.h +++ b/libraries/APO/APO.h @@ -8,6 +8,7 @@ #ifndef APO_H_ #define APO_H_ +#include #include "AP_Autopilot.h" #include "AP_Guide.h" #include "AP_Controller.h" @@ -20,6 +21,7 @@ #include "AP_ArmingMechanism.h" #include "AP_CommLink.h" #include "AP_Var_keys.h" +#include "DcmNavigator.h" #include "constants.h" #endif /* APO_H_ */ diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h index 407ae0f561..e183f8bdba 100644 --- a/libraries/APO/APO_DefaultSetup.h +++ b/libraries/APO/APO_DefaultSetup.h @@ -16,7 +16,7 @@ void setup() { using namespace apo; - AP_Var::load_all(); + Wire.begin(); // Declare all parts of the system AP_Navigator * navigator = NULL; @@ -36,23 +36,6 @@ void setup() { hal->debug = &Serial; hal->debug->println_P(PSTR("initializing debug line")); - // isr - hal->isr_registry = new Arduino_Mega_ISR_Registry; - - // initialize radio - hal->radio = new APM_RC_APM1; - hal->radio->Init(hal->isr_registry); - - // initialize scheduler - hal->scheduler = new AP_TimerProcess; - hal->scheduler->init(hal->isr_registry); - - - // initialize the adc - hal->debug->println_P(PSTR("initializing adc")); - hal->adc = new ADC_CLASS; - hal->adc->Init(hal->scheduler); - /* * Sensor initialization */ @@ -74,11 +57,14 @@ void setup() { if (baroEnabled) { hal->debug->println_P(PSTR("initializing baro")); hal->baro = new BARO_CLASS; - hal->baro->Init(); + if (board==BOARD_ARDUPILOTMEGA_2) { + hal->baro->Init(0,true); + } else { + hal->baro->Init(0,false); + } } if (compassEnabled) { - Wire.begin(); hal->debug->println_P(PSTR("initializing compass")); hal->compass = new COMPASS_CLASS; hal->compass->set_orientation(compassOrientation); @@ -155,7 +141,11 @@ void setup() { /* * Select guidance, navigation, control algorithms */ - navigator = new NAVIGATOR_CLASS(hal); + if (hal->getMode() == MODE_LIVE) { + navigator = new NAVIGATOR_CLASS(hal,k_nav); + } else { + navigator = new AP_Navigator(hal); + } guide = new GUIDE_CLASS(navigator, hal, velCmd, xt, xtLim); controller = new CONTROLLER_CLASS(navigator,guide,hal); diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index af773db570..3366eddfec 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -49,6 +49,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, * Look for valid initial state */ while (_navigator) { + // letc gcs known we are alive hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); @@ -98,10 +99,11 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, AP_MavlinkCommand::home.getLat()*rad2Deg, AP_MavlinkCommand::home.getLon()*rad2Deg, AP_MavlinkCommand::home.getCommand()); + guide->setCurrentIndex(0); controller->setMode(MAV_MODE_LOCKED); controller->setState(MAV_STATE_STANDBY); - + /* * Attach loops, stacking for priority */ @@ -117,7 +119,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, void AP_Autopilot::callback(void * data) { AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->hal()->debug->println_P(PSTR("callback")); + //apo->getHal()->debug->println_P(PSTR("callback")); /* * ahrs update @@ -178,6 +180,7 @@ void AP_Autopilot::callback1(void * data) { */ if (apo->getHal()->gcs) { apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE); + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION); } /* @@ -218,11 +221,10 @@ void AP_Autopilot::callback2(void * data) { */ if (apo->getHal()->gcs) { // send messages - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW); + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW_INT); apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW); - //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION); - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION); + //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION); apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU); } diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp index f0ab7cec4e..f7760044ec 100644 --- a/libraries/APO/AP_CommLink.cpp +++ b/libraries/APO/AP_CommLink.cpp @@ -116,21 +116,21 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { case MAVLINK_MSG_ID_GPS_RAW: { mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(), - _navigator->getLat() * rad2Deg, - _navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0, - _navigator->getGroundSpeed()*1000.0, - _navigator->getYaw() * rad2Deg); + _hal->gps->latitude/1.0e7, + _hal->gps->longitude/1.0e7, _hal->gps->altitude/10.0, 0, 0, + _hal->gps->ground_speed*10.0, + _hal->gps->ground_course*10.0); break; } - /* - case MAVLINK_MSG_ID_GPS_RAW_INT: { - mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(), - _navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0, - _navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg); - break; + case MAVLINK_MSG_ID_GPS_RAW_INT: { + mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(), + _hal->gps->latitude, + _hal->gps->longitude, _hal->gps->altitude*10.0, 0, 0, + _hal->gps->ground_speed*10.0, + _hal->gps->ground_course*10.0); + break; } - */ case MAVLINK_MSG_ID_SCALED_IMU: { int16_t xmag, ymag, zmag; diff --git a/libraries/APO/AP_HardwareAbstractionLayer.cpp b/libraries/APO/AP_HardwareAbstractionLayer.cpp index 9939e90e57..ee8804811d 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.cpp +++ b/libraries/APO/AP_HardwareAbstractionLayer.cpp @@ -5,5 +5,95 @@ * Author: jgoppert */ + +// Libraries +#include +#include #include "AP_HardwareAbstractionLayer.h" +#include +#include +#include +#include + +namespace apo { + +// default ctors on pointers called on pointers here, this +// allows NULL to be used as a boolean for if the device was +// initialized +AP_HardwareAbstractionLayer::AP_HardwareAbstractionLayer(halMode_t mode, board_t board, MAV_TYPE vehicle) : + adc(), gps(), baro(), compass(), rangeFinders(), imu(), batteryMonitor(), + radio(), rc(), gcs(), + hil(), debug(), load(), _mode(mode), + _board(board), _vehicle(vehicle) { + + + AP_Var::load_all(); + + /* + * Board specific hardware initialization + */ + if (board == BOARD_ARDUPILOTMEGA_1280) { + slideSwitchPin = 40; + pushButtonPin = 41; + aLedPin = 37; + bLedPin = 36; + cLedPin = 35; + eepromMaxAddr = 1024; + 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 + adc = new AP_ADC_ADS7844; + radio = new APM_RC_APM1; + } else if (board == BOARD_ARDUPILOTMEGA_2560) { + 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 + adc = new AP_ADC_ADS7844; + radio = new APM_RC_APM1; + } 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 + /// FIXME adc = new ? + adc = new AP_ADC_ADS7844; + radio = new APM_RC_APM2; + } + + // isr + isr_registry = new Arduino_Mega_ISR_Registry; + + // initialize radio + radio->Init(isr_registry); + + // initialize scheduler + scheduler = new AP_TimerProcess; + scheduler->init(isr_registry); + + // initialize the adc + adc->Init(scheduler); +} + +} // apo + // vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h index 8ee730ff2c..43770431e8 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -47,56 +47,7 @@ public: // default ctors on pointers called on pointers here, this // allows NULL to be used as a boolean for if the device was // initialized - AP_HardwareAbstractionLayer(halMode_t mode, board_t board, MAV_TYPE vehicle) : - adc(), gps(), baro(), compass(), rangeFinders(), imu(), batteryMonitor(), - radio(), rc(), gcs(), - hil(), debug(), load(), _mode(mode), - _board(board), _vehicle(vehicle) { - - /* - * Board specific hardware initialization - */ - if (board == BOARD_ARDUPILOTMEGA_1280) { - slideSwitchPin = 40; - pushButtonPin = 41; - aLedPin = 37; - bLedPin = 36; - cLedPin = 35; - eepromMaxAddr = 1024; - 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 - } else if (board == BOARD_ARDUPILOTMEGA_2560) { - 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 - } 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 - } - } + AP_HardwareAbstractionLayer(halMode_t mode, board_t board, MAV_TYPE vehicle); /** * Sensors diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index 439bcd3f2e..4ce5791f7b 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -5,20 +5,8 @@ * Author: jgoppert */ -#include "../FastSerial/FastSerial.h" #include "AP_Navigator.h" -#include "AP_CommLink.h" -#include "AP_HardwareAbstractionLayer.h" -#include "../AP_DCM/AP_DCM.h" -#include "../AP_Math/AP_Math.h" -#include "../AP_Compass/AP_Compass.h" #include "AP_MavlinkCommand.h" -#include "AP_Var_keys.h" -#include "../AP_RangeFinder/AP_RangeFinder.h" -#include "../AP_IMU/AP_IMU.h" -#include "../AP_InertialSensor/AP_InertialSensor.h" -#include "../APM_BMP085/APM_BMP085_hil.h" -#include "../APM_BMP085/APM_BMP085.h" namespace apo { @@ -29,8 +17,6 @@ AP_Navigator::AP_Navigator(AP_HardwareAbstractionLayer * hal) : _vN(0), _vE(0), _vD(0), _lat_degInt(0), _lon_degInt(0), _alt_intM(0) { } -void AP_Navigator::calibrate() { -} float AP_Navigator::getPD() const { return AP_MavlinkCommand::home.getPD(getAlt_intM()); } @@ -55,217 +41,5 @@ void AP_Navigator::setPN(float _pN) { setLon(AP_MavlinkCommand::home.getLon(_pN)); } -DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal) : - AP_Navigator(hal), _dcm(), _imuOffsetAddress(0) { - - // if orientation equal to front, store as front - /** - * rangeFinder is assigned values based on orientation which - * is specified in ArduPilotOne.pde. - */ - for (uint8_t i = 0; i < _hal-> rangeFinders.getSize(); i++) { - if (_hal->rangeFinders[i] == NULL) - continue; - if (_hal->rangeFinders[i]->orientation_x == 0 - && _hal->rangeFinders[i]->orientation_y == 0 - && _hal->rangeFinders[i]->orientation_z == 1) - _rangeFinderDown = _hal->rangeFinders[i]; - } - - if (_hal->getMode() == MODE_LIVE) { - - 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); - } - } -} -void DcmNavigator::calibrate() { - - AP_Navigator::calibrate(); - - // TODO: handle cold/warm restart - if (_hal->imu) { - _hal->imu->init(IMU::COLD_START,delay,_hal->scheduler); - } - - if (_hal->baro) { - // XXX should be moved to hal ctor - _hal->baro->Init(1,false); - // for apm 2 _hal->baro->Init(1,true); - int flashcount = 0; - - while(getGroundPressure() == 0){ - _hal->baro->Read(); // Get initial data from absolute pressure sensor - setGroundPressure(_hal->baro->Press); - setGroundTemperature(_hal->baro->Temp); - //mavlink_delay(20); - delay(20); - //Serial.printf("_hal->baro->Press %ld\n", _hal->baro->Press); - } - - for(int i = 0; i < 30; i++){ // We take some readings... - - //#if HIL_MODE == HIL_MODE_SENSORS - //gcs_update(); // look for inbound hil packets - //#endif - - _hal->baro->Read(); // Get initial data from absolute pressure sensor - setGroundPressure((getGroundPressure() * 9l + _hal->baro->Press) / 10l); - setGroundTemperature((getGroundTemperature() * 9 + _hal->baro->Temp) / 10); - - //mavlink_delay(20); - delay(20); - if(flashcount == 5) { - digitalWrite(_hal->cLedPin, LOW); - digitalWrite(_hal->aLedPin, HIGH); - digitalWrite(_hal->bLedPin, LOW); - } - - if(flashcount >= 10) { - flashcount = 0; - digitalWrite(_hal->cLedPin, LOW); - digitalWrite(_hal->aLedPin, HIGH); - digitalWrite(_hal->bLedPin, LOW); - } - flashcount++; - } - - // TODO implement - //saveGroundPressure(); - //saveGroundTemperature(); - // - _hal->debug->printf_P(PSTR("abs_pressure %ld\n"),getGroundPressure()); - _hal->gcs->sendText(SEVERITY_LOW, PSTR("barometer calibration complete\n")); - } -} - -void DcmNavigator::updateFast(float dt) { - - if (_hal->getMode() != MODE_LIVE) - return; - - setTimeStamp(micros()); // if running in live mode, record new time stamp - - - //_hal->debug->println_P(PSTR("nav loop")); - - /** - * The altitued is read off the barometer by implementing the following formula: - * altitude (in m) = 44330*(1-(p/po)^(1/5.255)), - * where, po is pressure in Pa at sea level (101325 Pa). - * See http://www.sparkfun.com/tutorials/253 or type this formula - * in a search engine for more information. - * altInt contains the altitude in meters. - */ - if (_hal->baro) { - - if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695) - setAlt(_rangeFinderDown->distance); - - else { - float x, scaling, temp; - - _hal->baro->Read(); // Get new data from absolute pressure sensor - - float abs_pressure = getGroundPressure() * .7 + _hal->baro->Press * .3; // large filtering - scaling = getGroundPressure()/abs_pressure; - temp = getGroundTemperature() + 273.15f; - x = log(scaling) * temp * 29271.267f; - //Barometer Debug - //_hal->debug->printf_P(PSTR("Ground Pressure %f\tAbsolute Pressure = %f\tGround Temperature = %f\tscaling= %f\n"),getGroundPressure(),abs_pressure,temp,log(scaling)); - setAlt_intM(x / 10); - } - } - - // dcm class for attitude - if (_dcm) { - _dcm->update_DCM_fast(); - setRoll(_dcm->roll); - setPitch(_dcm->pitch); - setYaw(_dcm->yaw); - setRollRate(_dcm->get_gyro().x); - setPitchRate(_dcm->get_gyro().y); - setYawRate(_dcm->get_gyro().z); - - /* - * accel/gyro debug - */ - /* - Vector3f accel = _hal->imu->get_accel(); - Vector3f gyro = _hal->imu->get_gyro(); - Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"), - accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z); - */ - } -} -void DcmNavigator::updateSlow(float dt) { - if (_hal->getMode() != MODE_LIVE) - return; - - setTimeStamp(micros()); // if running in live mode, record new time stamp - - if (_hal->gps) { - _hal->gps->update(); - updateGpsLight(); - if (_hal->gps->fix && _hal->gps->new_data) { - setLat_degInt(_hal->gps->latitude); - setLon_degInt(_hal->gps->longitude); - //Preferential Barometer Altitude - if (_hal->baro) { - setAlt_intM(getAlt_intM()); - } - else { - setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm - } - setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s - } - } - - if (_hal->compass) { - _hal->compass->read(); - _hal->compass->calculate(_dcm->get_dcm_matrix()); - _hal->compass->null_offsets(_dcm->get_dcm_matrix()); - //_hal->debug->printf_P(PSTR("heading: %f"), _hal->compass->heading); - } -} -void DcmNavigator::updateGpsLight(void) { - // GPS LED on if we have a fix or Blink GPS LED if we are receiving data - // --------------------------------------------------------------------- - static bool GPS_light = false; - switch (_hal->gps->status()) { - case (2): - //digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. - break; - - case (1): - if (_hal->gps->valid_read == true) { - GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock - if (GPS_light) { - digitalWrite(_hal->cLedPin, LOW); - } else { - digitalWrite(_hal->cLedPin, HIGH); - } - _hal->gps->valid_read = false; - } - break; - - default: - digitalWrite(_hal->cLedPin, LOW); - break; - } -} - } // namespace apo // vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index b450ee9052..52569c167e 100644 --- a/libraries/APO/AP_Navigator.h +++ b/libraries/APO/AP_Navigator.h @@ -22,10 +22,6 @@ #include "constants.h" #include -class RangeFinder; -class IMU; -class AP_DCM; - namespace apo { class AP_HardwareAbstractionLayer; @@ -34,9 +30,14 @@ class AP_HardwareAbstractionLayer; class AP_Navigator { public: AP_Navigator(AP_HardwareAbstractionLayer * hal); - virtual void calibrate(); - virtual void updateFast(float dt) = 0; - virtual void updateSlow(float dt) = 0; + + // note, override these with derived navigator functionality + virtual void calibrate() {}; + virtual void updateFast(float dt) {}; + virtual void updateSlow(float dt) {}; + + + // accessors float getPD() const; float getPE() const; float getPN() const; @@ -85,6 +86,7 @@ public: float getLon() const { return _lon_degInt * degInt2Rad; + } void setLon(float _lon) { @@ -155,6 +157,7 @@ public: return y; } + float getSpeedOverGround() const { return sqrt(getVN()*getVN()+getVE()*getVE()); } @@ -264,22 +267,6 @@ public: _windSpeed = windSpeed; } - float getGroundPressure() const { - return _groundPressure; - } - - void setGroundPressure(long groundPressure) { - _groundPressure = groundPressure; - } - - float getGroundTemperature() const { - return _groundTemperature; - } - - void setGroundTemperature(long groundTemperature) { - _groundTemperature = groundTemperature; - } - protected: AP_HardwareAbstractionLayer * _hal; private: @@ -302,27 +289,6 @@ private: int32_t _lat_degInt; // deg / 1e7 int32_t _lon_degInt; // deg / 1e7 int32_t _alt_intM; // meters / 1e3 - float _groundTemperature; // XXX units? - float _groundPressure; // XXX units? -}; - -class DcmNavigator: public AP_Navigator { -private: - /** - * Sensors - */ - - RangeFinder * _rangeFinderDown; - AP_DCM * _dcm; - IMU * _imu; - uint16_t _imuOffsetAddress; - -public: - DcmNavigator(AP_HardwareAbstractionLayer * hal); - virtual void calibrate(); - virtual void updateFast(float dt); - virtual void updateSlow(float dt); - void updateGpsLight(void); }; } // namespace apo diff --git a/libraries/APO/AP_Var_keys.h b/libraries/APO/AP_Var_keys.h index 40ebf99620..01487c7141 100644 --- a/libraries/APO/AP_Var_keys.h +++ b/libraries/APO/AP_Var_keys.h @@ -8,6 +8,7 @@ enum keys { k_cntrl, k_guide, k_sensorCalib, + k_nav, k_radioChannelsStart=10, diff --git a/libraries/APO/DcmNavigator.cpp b/libraries/APO/DcmNavigator.cpp new file mode 100644 index 0000000000..537877f120 --- /dev/null +++ b/libraries/APO/DcmNavigator.cpp @@ -0,0 +1,217 @@ +/* + * DcmNavigator.cpp + * + * Created on: Dec 6, 2011 + * Author: jgoppert/ wenyaoxie + */ + +#include "../FastSerial/FastSerial.h" +#include "DcmNavigator.h" +#include "AP_CommLink.h" +#include "AP_HardwareAbstractionLayer.h" +#include "../AP_DCM/AP_DCM.h" +#include "../AP_Math/AP_Math.h" +#include "../AP_Compass/AP_Compass.h" +#include "AP_MavlinkCommand.h" +#include "AP_Var_keys.h" +#include "../AP_RangeFinder/RangeFinder.h" +#include "../AP_IMU/AP_IMU.h" +#include "../AP_InertialSensor/AP_InertialSensor.h" +#include "../APM_BMP085/APM_BMP085_hil.h" +#include "../APM_BMP085/APM_BMP085.h" + +namespace apo { + +DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal, const uint16_t key, const prog_char_t * name) : + AP_Navigator(hal), _imuOffsetAddress(0), + _dcm(_hal->imu, _hal->gps, _hal->compass), + _rangeFinderDown(), + _group(key, name ? : PSTR("NAV_")), + _baroLowPass(&_group,1,10,PSTR("BAROLP")), + _groundTemperature(&_group,2, 25.0,PSTR("GNDT")), // TODO check temp + _groundPressure(&_group,3,0.0,PSTR("GNDP")) { + + // if orientation equal to front, store as front + /** + * rangeFinder is assigned values based on orientation which + * is specified in ArduPilotOne.pde. + */ + for (uint8_t i = 0; i < _hal-> rangeFinders.getSize(); i++) { + if (_hal->rangeFinders[i] == NULL) + continue; + if (_hal->rangeFinders[i]->orientation_x == 0 + && _hal->rangeFinders[i]->orientation_y == 0 + && _hal->rangeFinders[i]->orientation_z == 1) + _rangeFinderDown = _hal->rangeFinders[i]; + } + + // 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); + } +} +void DcmNavigator::calibrate() { + + AP_Navigator::calibrate(); + + // TODO: handle cold/warm restart + if (_hal->imu) { + _hal->imu->init(IMU::COLD_START,delay,_hal->scheduler); + } + + if (_hal->baro) { + + int flashcount = 0; + + while(_groundPressure == 0){ + _hal->baro->Read(); // Get initial data from absolute pressure sensor + _groundPressure = _hal->baro->Press; + _groundTemperature = _hal->baro->Temp/10.0; + delay(20); + } + + for(int i = 0; i < 30; i++){ // We take some readings... + + // set using low pass filters + _groundPressure = _groundPressure * 0.9 + _hal->baro->Press * 0.1; + _groundTemperature = _groundTemperature * 0.9 + (_hal->baro->Temp/10.0) * 0.1; + + //mavlink_delay(20); + delay(20); + if(flashcount == 5) { + digitalWrite(_hal->cLedPin, LOW); + digitalWrite(_hal->aLedPin, HIGH); + digitalWrite(_hal->bLedPin, LOW); + } + + if(flashcount >= 10) { + flashcount = 0; + digitalWrite(_hal->cLedPin, LOW); + digitalWrite(_hal->aLedPin, HIGH); + digitalWrite(_hal->bLedPin, LOW); + } + flashcount++; + } + + _groundPressure.save(); + _groundTemperature.save(); + + _hal->debug->printf_P(PSTR("ground pressure: %ld ground temperature: %d\n"),_groundPressure.get(), _groundTemperature.get()); + _hal->gcs->sendText(SEVERITY_LOW, PSTR("barometer calibration complete\n")); + } +} + +void DcmNavigator::updateFast(float dt) { + + if (_hal->getMode() != MODE_LIVE) + return; + + setTimeStamp(micros()); // if running in live mode, record new time stamp + + // use range finder if attached and close to the ground + if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695) { + setAlt(_rangeFinderDown->distance); + + // otherwise if you have a baro attached, use it + } else if (_hal->baro) { + /** + * The altitued is read off the barometer by implementing the following formula: + * altitude (in m) = 44330*(1-(p/po)^(1/5.255)), + * where, po is pressure in Pa at sea level (101325 Pa). + * See http://www.sparkfun.com/tutorials/253 or type this formula + * in a search engine for more information. + * altInt contains the altitude in meters. + * + * pressure input is in pascals + * temp input is in deg C *10 + */ + _hal->baro->Read(); // Get new data from absolute pressure sensor + float reference = 44330 * (1.0 - (pow(_groundPressure.get()/101325.0,0.190295))); + setAlt(_baroLowPass.update((44330 * (1.0 - (pow((_hal->baro->Press/101325.0),0.190295)))) - reference,dt)); + _hal->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_hal->baro->Press,_hal->baro->Temp); + + // last resort, use gps altitude + } else if (_hal->gps && _hal->gps->fix) { + setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm + } + + // update dcm calculations and navigator data + // + _dcm.update_DCM_fast(); + setRoll(_dcm.roll); + setPitch(_dcm.pitch); + setYaw(_dcm.yaw); + setRollRate(_dcm.get_gyro().x); + setPitchRate(_dcm.get_gyro().y); + setYawRate(_dcm.get_gyro().z); + + /* + * accel/gyro debug + */ + /* + Vector3f accel = _hal->imu->get_accel(); + Vector3f gyro = _hal->imu->get_gyro(); + Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"), + accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z); + */ +} + +void DcmNavigator::updateSlow(float dt) { + if (_hal->getMode() != MODE_LIVE) + return; + + setTimeStamp(micros()); // if running in live mode, record new time stamp + + if (_hal->gps) { + _hal->gps->update(); + updateGpsLight(); + if (_hal->gps->fix && _hal->gps->new_data) { + setLat_degInt(_hal->gps->latitude); + setLon_degInt(_hal->gps->longitude); + setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s + } + } + + if (_hal->compass) { + _hal->compass->read(); + _hal->compass->calculate(_dcm.get_dcm_matrix()); + _hal->compass->null_offsets(_dcm.get_dcm_matrix()); + //_hal->debug->printf_P(PSTR("heading: %f"), _hal->compass->heading); + } +} +void DcmNavigator::updateGpsLight(void) { + // GPS LED on if we have a fix or Blink GPS LED if we are receiving data + // --------------------------------------------------------------------- + static bool GPS_light = false; + switch (_hal->gps->status()) { + case (2): + //digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. + break; + + case (1): + if (_hal->gps->valid_read == true) { + GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (GPS_light) { + digitalWrite(_hal->cLedPin, LOW); + } else { + digitalWrite(_hal->cLedPin, HIGH); + } + _hal->gps->valid_read = false; + } + break; + + default: + digitalWrite(_hal->cLedPin, LOW); + break; + } +} + +} // namespace apo +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/DcmNavigator.h b/libraries/APO/DcmNavigator.h new file mode 100644 index 0000000000..e3a308fdae --- /dev/null +++ b/libraries/APO/DcmNavigator.h @@ -0,0 +1,51 @@ +/* + * DcmNavigator.h + * Copyright (C) James Goppert/ Wenyao Xie 2011 james.goppert@gmail.com/ wenyaoxie@gmail.com + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef DcmNavigator_H +#define DcmNavigator_H + +#include "AP_Navigator.h" +#include +#include "AP_ControllerBlock.h" +#include + +class RangeFinder; + +namespace apo { + +class DcmNavigator: public AP_Navigator { +public: + DcmNavigator(AP_HardwareAbstractionLayer * hal, const uint16_t key, const prog_char_t * name = NULL); + virtual void calibrate(); + virtual void updateFast(float dt); + virtual void updateSlow(float dt); + void updateGpsLight(void); +private: + AP_DCM _dcm; + AP_Var_group _group; + uint16_t _imuOffsetAddress; + BlockLowPass _baroLowPass; + AP_Float _groundTemperature; + AP_Float _groundPressure; + RangeFinder * _rangeFinderDown; +}; + +} // namespace apo + +#endif // DcmNavigator_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/template.h b/libraries/APO/template.h index 0ebf3dd191..765d1fe5bb 100644 --- a/libraries/APO/template.h +++ b/libraries/APO/template.h @@ -24,7 +24,7 @@ namespace apo { /// Class description class Class { public: -} +}; } // namespace apo diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 95a11c5bb4..5d813d793c 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -173,7 +173,10 @@ void AP_DCM::accel_adjust(void) { Vector3f veloc, temp; - veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units + + if (_gps) { + veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units + } // We are working with a modified version of equation 26 as our IMU object reports acceleration in the positive axis direction as positive @@ -301,7 +304,7 @@ AP_DCM::drift_correction(void) // We make the gyro YAW drift correction based on compass magnetic heading error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x); // Equation 23, Calculating YAW error - } else { + } else if (_gps) { // Use GPS Ground course to correct yaw gyro drift if (_gps->ground_speed >= SPEEDFILT) { From c4203631dca095b7a90dc6e328f26c998fd6509d Mon Sep 17 00:00:00 2001 From: Wenyao Xie Date: Tue, 6 Dec 2011 19:38:23 -0500 Subject: [PATCH 5/5] APO velocity/ altitude working. --- apo/QuadArducopter.h | 2 +- libraries/APO/AP_Autopilot.cpp | 4 ++-- libraries/APO/AP_CommLink.cpp | 10 +++++----- libraries/APO/DcmNavigator.cpp | 5 ++++- 4 files changed, 12 insertions(+), 9 deletions(-) diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 915483b55d..29464038c0 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -55,7 +55,7 @@ static const bool rangeFinderUpEnabled = false; static const bool rangeFinderDownEnabled = false; // loop rates -static const float loopRate = 250; // attitude nav +static const float loopRate = 200; // attitude nav static const float loop0Rate = 50; // controller static const float loop1Rate = 10; // pos nav/ gcs fast static const float loop2Rate = 1; // gcs slow diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index 3366eddfec..fcd5fef57e 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -221,10 +221,10 @@ void AP_Autopilot::callback2(void * data) { */ if (apo->getHal()->gcs) { // send messages - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW_INT); + //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW_INT); + //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION); apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW); - //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION); apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU); } diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp index f7760044ec..d7c3beee55 100644 --- a/libraries/APO/AP_CommLink.cpp +++ b/libraries/APO/AP_CommLink.cpp @@ -117,9 +117,9 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { case MAVLINK_MSG_ID_GPS_RAW: { mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(), _hal->gps->latitude/1.0e7, - _hal->gps->longitude/1.0e7, _hal->gps->altitude/10.0, 0, 0, - _hal->gps->ground_speed*10.0, - _hal->gps->ground_course*10.0); + _hal->gps->longitude/1.0e7, _hal->gps->altitude/100.0, 0, 0, + _hal->gps->ground_speed/100.0, + _hal->gps->ground_course/10.0); break; } @@ -127,8 +127,8 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(), _hal->gps->latitude, _hal->gps->longitude, _hal->gps->altitude*10.0, 0, 0, - _hal->gps->ground_speed*10.0, - _hal->gps->ground_course*10.0); + _hal->gps->ground_speed/100.0, + _hal->gps->ground_course/10.0); break; } diff --git a/libraries/APO/DcmNavigator.cpp b/libraries/APO/DcmNavigator.cpp index 537877f120..71f75ac359 100644 --- a/libraries/APO/DcmNavigator.cpp +++ b/libraries/APO/DcmNavigator.cpp @@ -135,7 +135,7 @@ void DcmNavigator::updateFast(float dt) { _hal->baro->Read(); // Get new data from absolute pressure sensor float reference = 44330 * (1.0 - (pow(_groundPressure.get()/101325.0,0.190295))); setAlt(_baroLowPass.update((44330 * (1.0 - (pow((_hal->baro->Press/101325.0),0.190295)))) - reference,dt)); - _hal->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_hal->baro->Press,_hal->baro->Temp); + //_hal->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_hal->baro->Press,_hal->baro->Temp); // last resort, use gps altitude } else if (_hal->gps && _hal->gps->fix) { @@ -151,6 +151,9 @@ void DcmNavigator::updateFast(float dt) { setRollRate(_dcm.get_gyro().x); setPitchRate(_dcm.get_gyro().y); setYawRate(_dcm.get_gyro().z); + setXAccel(_dcm.get_accel().x); + setYAccel(_dcm.get_accel().y); + setZAccel(_dcm.get_accel().z); /* * accel/gyro debug