mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of github.com:arktools/ardupilotone
This commit is contained in:
commit
fb3ab2c8d0
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
@ -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
|
||||
|
|
|
@ -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_ */
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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_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_GLOBAL_POSITION);
|
||||
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION);
|
||||
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
|
||||
}
|
||||
|
||||
|
|
|
@ -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/100.0, 0, 0,
|
||||
_hal->gps->ground_speed/100.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/100.0,
|
||||
_hal->gps->ground_course/10.0);
|
||||
break;
|
||||
}
|
||||
*/
|
||||
|
||||
case MAVLINK_MSG_ID_SCALED_IMU: {
|
||||
int16_t xmag, ymag, zmag;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -6,18 +6,7 @@
|
|||
*/
|
||||
|
||||
#include "AP_Navigator.h"
|
||||
#include "../FastSerial/FastSerial.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 {
|
||||
|
||||
|
@ -28,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());
|
||||
}
|
||||
|
@ -54,153 +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);
|
||||
}
|
||||
}
|
||||
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 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);
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
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
|
||||
|
|
|
@ -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,7 +267,6 @@ public:
|
|||
_windSpeed = windSpeed;
|
||||
}
|
||||
|
||||
|
||||
protected:
|
||||
AP_HardwareAbstractionLayer * _hal;
|
||||
private:
|
||||
|
@ -289,25 +291,6 @@ private:
|
|||
int32_t _alt_intM; // meters / 1e3
|
||||
};
|
||||
|
||||
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
|
||||
|
||||
#endif // AP_Navigator_H
|
||||
|
|
|
@ -8,6 +8,7 @@ enum keys {
|
|||
k_cntrl,
|
||||
k_guide,
|
||||
k_sensorCalib,
|
||||
k_nav,
|
||||
|
||||
k_radioChannelsStart=10,
|
||||
|
||||
|
|
|
@ -0,0 +1,220 @@
|
|||
/*
|
||||
* 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);
|
||||
setXAccel(_dcm.get_accel().x);
|
||||
setYAccel(_dcm.get_accel().y);
|
||||
setZAccel(_dcm.get_accel().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
|
|
@ -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
|
|
@ -24,7 +24,7 @@ namespace apo {
|
|||
/// Class description
|
||||
class Class {
|
||||
public:
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace apo
|
||||
|
||||
|
|
|
@ -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) {
|
||||
|
|
Loading…
Reference in New Issue