Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fixed baro.

This commit is contained in:
Wenyao Xie 2011-12-06 18:56:16 -05:00
parent c26956af19
commit 9fef8689d8
15 changed files with 408 additions and 362 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduBoat/BoatGeneric.h
View File

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

2
apo/QuadArducopter.h
View File

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

2
libraries/APO/APO.h
View File

@ -8,6 +8,7 @@
#ifndef APO_H_
#define APO_H_
#include <FastSerial.h>
#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_ */

32
libraries/APO/APO_DefaultSetup.h
View File

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

12
libraries/APO/AP_Autopilot.cpp
View File

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

22
libraries/APO/AP_CommLink.cpp
View File

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

90
libraries/APO/AP_HardwareAbstractionLayer.cpp
View File

@ -5,5 +5,95 @@
* Author: jgoppert
*/
// Libraries
#include <FastSerial.h>
#include <Arduino_Mega_ISR_Registry.h>
#include "AP_HardwareAbstractionLayer.h"
#include <AP_ADC.h>
#include <APM_RC.h>
#include <AP_AnalogSource.h>
#include <AP_PeriodicProcess.h>
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

51
libraries/APO/AP_HardwareAbstractionLayer.h
View File

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

226
libraries/APO/AP_Navigator.cpp
View File

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

54
libraries/APO/AP_Navigator.h
View File

@ -22,10 +22,6 @@
#include "constants.h"
#include <inttypes.h>
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

1
libraries/APO/AP_Var_keys.h
View File

@ -8,6 +8,7 @@ enum keys {
k_cntrl,
k_guide,
k_sensorCalib,
k_nav,
k_radioChannelsStart=10,

217
libraries/APO/DcmNavigator.cpp Normal file
View File

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

51
libraries/APO/DcmNavigator.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#ifndef DcmNavigator_H
#define DcmNavigator_H
#include "AP_Navigator.h"
#include <FastSerial.h>
#include "AP_ControllerBlock.h"
#include <AP_DCM.h>
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

2
libraries/APO/template.h
View File

@ -24,7 +24,7 @@ namespace apo {
/// Class description
class Class {
public:
}
};
} // namespace apo

7
libraries/AP_DCM/AP_DCM.cpp
View File

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