ardupilot/libraries/APO/AP_Navigator.cpp

/*
 * AP_Navigator.cpp
 *
 *  Created on: Apr 30, 2011
 *      Author: jgoppert
 */

#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 "../APM_BMP085/APM_BMP085.h"

namespace apo {

AP_Navigator::AP_Navigator(AP_HardwareAbstractionLayer * hal) :
	_hal(hal), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0),
			_pitchRate(0), _yaw(0), _yawRate(0), _airSpeed(0),
			_groundSpeed(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());
}

float AP_Navigator::getPE() const {
	return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt());
}

float AP_Navigator::getPN() const {
	return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt());
}

void AP_Navigator::setPD(float _pD) {
	setAlt(AP_MavlinkCommand::home.getAlt(_pD));
}

void AP_Navigator::setPE(float _pE) {
	setLat(AP_MavlinkCommand::home.getLat(_pE));
}

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->adc)
			_hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib);
		if (_hal->imu)
			_dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass);
		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);
	}
}
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